Initial commit

include/impl/moveit_robot.h

+#ifndef MOVEIT_ROBOT_
+#define MOVEIT_ROBOT_
+
+#include <ros/ros.h>
+#include "impl/robot.h"
+
+#include <moveit/planning_pipeline/planning_pipeline.h>
+#include <gb_grasp/MapGenerator.h>
+
+
+
+class Moveit_robot : public Robot{
+    protected:
+    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi_;
+    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi_hand_;
+
+    std::map<std::string, std::string>  map_; 
+    std::shared_ptr<MapGenerator> grasp_map_generator_;
+
+
+
+    public:
+    Moveit_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Robot(name, tf, size)
+        , mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>(name)){
+
+        std::stringstream hand_n, ik_frame_n, name_n;
+        hand_n << "hand_" << name.back();
+        ik_frame_n << "panda_" << name.back() << "_link8";
+        mgi_hand_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(hand_n.str());
+
+
+        map_.insert(std::make_pair<std::string, std::string>("right_finger", mgi_hand_->getLinkNames()[0].c_str()));
+        map_.insert(std::make_pair<std::string, std::string>("left_finger", mgi_hand_->getLinkNames()[1].c_str()));
+        map_.insert(std::make_pair<std::string, std::string>("hand_link", mgi_hand_->getLinkNames()[2].c_str()));
+        map_.insert(std::make_pair<std::string, std::string>("eef_name", hand_n.str()));
+        map_.insert(std::make_pair<std::string, std::string>("hand_frame", ik_frame_n.str()));
+        map_.insert(std::make_pair<std::string, std::string>("hand_group_name", hand_n.str()));
+    }
+
+    inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;}
+    inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi_hand() {return mgi_hand_;}
+
+    inline std::map<std::string, std::string>&  map(){return map_;}
+    inline void set_grasp_map_generator(std::shared_ptr<MapGenerator>const& d ){ grasp_map_generator_ = d;}
+};
+
+#endif
\ No newline at end of file

include/impl/robot.h

+#ifndef ROBOT_
+#define ROBOT_
+
+#include "ros/ros.h"
+#include "impl/abstract_robot.h"
+#include "impl/abstract_robot_element.h"
+#include "impl/abstract_robot_element_decorator.h"
+#include "impl/wing.h"
+#include "impl/wing_rviz_decorator.h"
+#include "impl/field.h"
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit_msgs/ApplyPlanningScene.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/kinematic_constraints/utils.h>
+
+class Robot : public Abstract_robot{
+    protected:
+    std::vector<Abstract_robot_element*> observers_;
+    std::vector<Abstract_robot_element*> access_fields_;
+    std::vector<moveit_msgs::CollisionObject*> coll_markers_;
+
+
+    public:
+    Robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Abstract_robot(name, tf, size){generate_access_fields();}
+    
+    inline std::vector<Abstract_robot_element*>& observers() { return observers_;}
+    inline std::vector<Abstract_robot_element*>& access_fields() { return access_fields_;}
+
+    inline void add_coll_markers(moveit_msgs::CollisionObject* marker) {coll_markers_.push_back(marker);}
+    inline std::vector<moveit_msgs::CollisionObject*> coll_markers(){ return coll_markers_;}
+
+    void register_observers(Abstract_robot_element* wd);
+    void reset();
+    void generate_access_fields();
+    float area_calculation(tf2::Transform& A, tf2::Transform& B, tf2::Transform& C);
+    bool check_robot_collision(Robot* R);
+
+    bool check_single_object_collision(tf2::Transform& obj, std::string& str) override;
+    void workload_checker(std::vector<int>& count_vector, tf2::Transform& obj) override;
+    void notify() override;
+
+
+};
+
+
+
+
+#endif
\ No newline at end of file

include/impl/wing.h

+#ifndef WING_
+#define WING_
+
+#include "ros/ros.h"
+#include "impl/abstract_robot_element.h"
+
+class Wing  : public Abstract_robot_element{
+    protected:
+    std::string name_;
+    tf2::Vector3 size_;
+    std::vector<tf2::Transform> bounds_;
+
+    public:
+    Wing(std::string name, tf2::Transform tf, tf2::Vector3 size) : size_(size), name_(name){ 
+        relative_tf_ = tf;
+        bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/2.f, size_.getY()/2.f,0))); //++
+        bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/2.f, size_.getY()/-2.f,0))); //+-
+        bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/-2.f, size_.getY()/-2.f,0))); //--
+        bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/-2.f, size_.getY()/2.f,0))); //-+
+        }
+    virtual ~Wing() {}
+        
+    inline std::string& name() {return name_;}
+    inline void set_name(std::string str) {name_ = str;}
+    inline void set_set(tf2::Vector3& vec) {size_ = vec;}
+
+    inline tf2::Vector3 size(){ return size_;}
+    inline std::vector<tf2::Transform>& bounds() {return bounds_;}
+    void update(tf2::Transform& tf) override {this->calc_world_tf(tf);}
+};
+
+#endif
\ No newline at end of file

include/impl/wing_moveit_decorator.h

+#ifndef WING_MOVEIT_DECORATOR_
+#define WING_MOVEIT_DECORATOR_
+
+#include <ros/ros.h>
+#include "impl/wing.h"
+#include "impl/abstract_robot_element_decorator.h"
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit_msgs/ApplyPlanningScene.h>
+#include <moveit/planning_scene/planning_scene.h>
+
+class Wing_moveit_decorator  : public Abstract_robot_element_decorator{
+    private:
+    moveit_msgs::CollisionObject* markers_ = new moveit_msgs::CollisionObject();
+
+
+    public:
+    Wing_moveit_decorator(Abstract_robot_element* next) : Abstract_robot_element_decorator(next){};
+    inline void set_markers(moveit_msgs::CollisionObject* markers) { markers_= markers;}
+    inline moveit_msgs::CollisionObject* markers() { return markers_;}
+    
+    void update(tf2::Transform& tf) override;
+    void input_filter(tf2::Transform& tf) override;
+    void output_filter() override;
+};
+
+
+#endif
\ No newline at end of file

include/impl/wing_rviz_decorator.h

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

include/reader/abstract_param_reader.h

+#ifndef ABSTRACT_PARAM_READER_
+#define ABSTRACT_PARAM_READER_
+
+#include "ros/ros.h"
+#include <ros/package.h>
+#include <xmlrpcpp/XmlRpc.h>
+#include <tf2/LinearMath/Transform.h>
+#include <yaml-cpp/yaml.h>
+#include <fstream>
+#include <regex>
+#include <yaml-cpp/node/detail/node_data.h>
+
+struct object_data {
+    std::string name_;
+    tf2::Transform pose_;
+    tf2::Vector3 size_;
+};
+
+class Abstract_param_reader{
+    protected:
+    std::shared_ptr<ros::NodeHandle> nh_;
+
+    public:
+    Abstract_param_reader(std::shared_ptr<ros::NodeHandle> const& d) : nh_(d){}
+
+    float float_of(XmlRpc::XmlRpcValue& val);
+    virtual void read()=0;
+};
+#endif

include/reader/cuboid_reader.h

+#ifndef CUBOID_READER_
+#define CUBOID_READER_
+
+#include "ros/ros.h"
+#include <ros/package.h>
+#include <xmlrpcpp/XmlRpc.h>
+#include <gb_grasp/Cuboid.h>
+
+
+#include "reader/abstract_param_reader.h"
+
+
+class Cuboid_reader : public Abstract_param_reader{
+    protected:
+    std::vector<Cuboid> cuboid_bin_;
+    std::vector<Cuboid> cuboid_obstacle_;
+
+    public:
+    Cuboid_reader(std::shared_ptr<ros::NodeHandle> const& d) : Abstract_param_reader(d){read();}
+    
+    inline void set_cuboid_bin_data(std::vector<Cuboid>& cuboid_data) {cuboid_bin_ = cuboid_data;}
+    inline void set_cuboid_obstacle_data(std::vector<Cuboid>& cuboid_data) {cuboid_obstacle_ = cuboid_data;}
+
+    inline std::vector<Cuboid> cuboid_bin() {return cuboid_bin_;}
+    inline std::vector<Cuboid> cuboid_obstacle() {return cuboid_obstacle_;}
+
+    void read() override;
+};
+#endif

include/reader/robot_reader.h

+#ifndef ROBOT_READER_
+#define ROBOT_READER_
+
+#include "ros/ros.h"
+#include <ros/package.h>
+#include <xmlrpcpp/XmlRpc.h>
+
+#include "reader/abstract_param_reader.h"
+
+
+class Robot_reader : public Abstract_param_reader{
+    protected:
+    std::vector<object_data> robot_data_;
+
+    public:
+    Robot_reader(std::shared_ptr<ros::NodeHandle> const& d) : Abstract_param_reader(d){read();}
+    
+    inline void set_robot_data(std::vector<object_data>& robot_data) {robot_data_ = robot_data;}
+    inline std::vector<object_data> robot_data() {return robot_data_;}
+
+    void read() override;
+};
+#endif

include/reader/task_space_reader.h

+#ifndef TASK_SPACE_READER_
+#define TASK_SPACE_READER_
+
+#include "ros/ros.h"
+#include <ros/package.h>
+#include <xmlrpcpp/XmlRpc.h>
+
+#include "reader/abstract_param_reader.h"
+
+
+class Task_space_reader : public Abstract_param_reader{
+    public:
+    Task_space_reader(std::shared_ptr<ros::NodeHandle> const& d) : Abstract_param_reader(d){read();}
+    
+    void read() override;
+};
+#endif

include/reader/wing_reader.h

+#ifndef WING_READER_
+#define WING_READER_
+
+#include "ros/ros.h"
+#include <ros/package.h>
+#include <xmlrpcpp/XmlRpc.h>
+
+#include "reader/abstract_param_reader.h"
+
+
+class Wing_reader : public Abstract_param_reader{
+    protected:
+    std::vector<std::pair<std::vector<object_data>, int>> wing_data_;
+
+    public:
+    Wing_reader(std::shared_ptr<ros::NodeHandle> const& d) : Abstract_param_reader(d){read();}
+    
+    inline void set_wing_data(std::vector<std::pair<std::vector<object_data>, int>>& wing_data) {wing_data_ = wing_data;}
+    inline std::vector<std::pair<std::vector<object_data>, int>> wing_data() {return wing_data_;}
+
+    void read() override;
+};
+#endif

launch/base_routine.launch

+<launch>
+    <arg name="map" default="dummy.yaml" />
+    <arg name="description" default="dummy.yaml" />
+    <arg name="resource" default="dummy" />
+
+
+    <rosparam command="load" file="$(find multi_cell_builder)/maps/$(arg map)"/>
+    <rosparam command="load" file="$(find multi_cell_builder)/descriptions/$(arg description)"/>
+    <rosparam command="load" file="$(find multi_cell_builder)/resources/$(arg resource).yaml"/>
+
+    <rosparam param="resource_name" subst_value="True"> $(arg resource)</rosparam>
+    <node pkg="multi_cell_builder" type="base_routine" name="base_routine" output="screen" required="true"/>
+</launch>
\ No newline at end of file

launch/cell_routine.launch

+<launch>    
+    <arg name="result" default="dummy/-562289182.yaml" />
+    <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
+
+    <!-- this is to change-->
+    <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
+    <rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
+    <rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
+    <rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/>
+
+    
+    <rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
+
+    <include file="$(find ceti_double)/launch/demo.launch"> 
+        <arg name="use_rviz" value="false"/>
+        <arg name="scene" value="$(arg result)" />
+    </include> 
+    
+    <include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" />
+    <param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" />
+
+    <include file="$(find ceti_double)/launch/moveit_rviz.launch">
+        <arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
+    </include>
+
+    <node pkg="multi_cell_builder" type="cell_routine" name="cell_routine" output="screen" required="true">
+    </node>
+</launch>
+
+    

launch/gdb_settings.gdb

+## This file allows developers to set breakpoints in the CPP_EXECUTABLE_NAME.launch file
+## To set a breakpoint, compile with debug flag set to true. Add a line below with the source file name,
+## a colon, then the line number to break on. Then launch CPP_EXECUTABLE_NAME with argument debug:=true
+
+set breakpoint pending on
+
+## Example break point:
+# break moveit_visual_tools.cpp:322

launch/mtc_auerswald.launch

+<launch>
+  <!-- Launch rviz.launch before running this demo to see visalizations -->
+  <arg name="result" default="dummy/-562289182.yaml"/>
+  <arg name="grasps_ns" default="grasps" />
+
+
+  <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" />
+
+
+
+  <!-- PANDA -->
+  <include file="$(find ceti_double)/launch/planning_context.launch">
+    <arg name="load_robot_description" value="true"/>
+    <arg name="scene" value="$(arg result)" />
+  </include>
+
+  <!-- this is needed because moveit grasps uses an additional joint -->
+  <arg name="robot_description" default="robot_description"/>
+  <param  name="$(arg robot_description)" command="$(find xacro)/xacro '$(find moveit_grasps)/robots/double_panda_arm_two_finger_hand.urdf.xacro' 'scene:=$(arg result)'"/>
+
+
+  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world base_1" />
+  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world base_2" />
+
+
+  <!-- Load Rviz -->
+  <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 ceti_double)/config/kinematics.yaml"/>
+  </node>
+
+  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <include file="$(find ceti_double)/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="true"/>
+    <arg name="pipeline" value="ompl"  />
+    <arg name="scene" value="$(arg result)" />
+  </include>
+
+  <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="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+
+  <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
+  <rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
+  <rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
+  <rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/>
+
+  <rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
+  <rosparam command="load" file="$(find gb_grasp)/config/grasp_map_config.yaml"/>
+
+  <node pkg="multi_cell_builder" type="grasp_cell_routine" name="grasp_cell_routine" output="screen" required="true">
+      <rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
+  </node>
+
+  <!-- Planning Functionality -->
+  <arg name="planner" default="ompl" />
+  <include ns="grasp_cell_routine" file="$(find ceti_double)/launch/planning_pipeline.launch.xml">
+    <arg name="pipeline" value="$(arg planner)" />
+  </include>
+
+</launch>

launch/rviz/cell_routine.rviz

+Panels:
+  - Class: rviz/Displays
+    Help Height: 84
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Motion Planning Tasks1
+      Splitter Ratio: 0.5393258333206177
+    Tree Height: 533
+  - Class: rviz/Help
+    Name: Help
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: moveit_task_constructor/Motion Planning Tasks
+    Global Settings:
+      Task View Settings:
+        Show Computation Times: true
+        Task Expansion: All Expanded
+    Name: Motion Planning Tasks
+    Tasks View:
+      property_splitter:
+        - 540
+        - 0
+      solution_sorting:
+        column: 1
+        order: 0
+      solutions_splitter:
+        - 306
+        - 98
+      solutions_view_columns:
+        - 38
+        - 52
+        - 0
+      tasks_view_columns:
+        - 226
+        - 38
+        - 38
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: moveit_rviz_plugin/PlanningScene
+      Enabled: true
+      Move Group Namespace: ""
+      Name: PlanningScene
+      Planning Scene Topic: move_group/monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.20000000298023224
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+    - Class: moveit_task_constructor/Motion Planning Tasks
+      Enabled: true
+      Interrupt Display: false
+      Loop Animation: false
+      Markers:
+        All at once?: false
+        Value: true
+      Name: Motion Planning Tasks
+      Robot:
+        Fixed Robot Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Use Fixed Robot Color: false
+        Value: ""
+      Robot Description: robot_description
+      Scene:
+        Attached Body Color: 150; 50; 150
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Value: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Show Trail: false
+      State Display Time: 0.05 s
+      Task Solution Topic: /cell_routine/solution # change to nodes name
+      Tasks:
+        Cartesian Path: 1
+      Trail Step Size: 1
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+  Value: true
+  Views:
+    Current:
+      Class: rviz/XYOrbit
+      Distance: 1.3878222703933716
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.30880630016326904
+        Y: -0.1259305477142334
+        Z: 1.3560062370743253e-6
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.264797568321228
+      Target Frame: world
+      Yaw: 4.939944744110107
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 768
+  Help:
+    collapsed: false
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Motion Planning Tasks:
+    collapsed: false
+  Motion Planning Tasks - Slider:
+    collapsed: false
+  QMainWindow State: 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
+  Views:
+    collapsed: false
+  Width: 1621
+  X: 249
+  Y: 25

launch/rviz/grasps.rviz

+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /MarkerArray1/Namespaces1
+      Splitter Ratio: 0.5704230070114136
+    Tree Height: 545
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+  - Class: rviz_visual_tools/RvizVisualToolsGui
+    Name: RvizVisualToolsGui
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: moveit_rviz_plugin/Trajectory
+      Color Enabled: false
+      Enabled: true
+      Interrupt Display: false
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        panda_hand:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_leftfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_rightfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tool_center_point:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Loop Animation: false
+      Name: Trajectory
+      Robot Alpha: 0.5
+      Robot Color: 150; 50; 150
+      Robot Description: robot_description
+      Show Robot Collision: false
+      Show Robot Visual: true
+      Show Trail: false
+      State Display Time: 0.05 s
+      Trail Step Size: 1
+      Trajectory Topic: /display_planned_path
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /rviz_visual_tools
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /grasp_visuals
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Attached Body Color: 150; 50; 150
+      Class: moveit_rviz_plugin/RobotState
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        panda_hand:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_leftfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_rightfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tool_center_point:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotState
+      Robot Alpha: 1
+      Robot Description: robot_description
+      Robot State Topic: /display_robot_state
+      Show All Links: true
+      Show Highlights: true
+      Value: true
+      Visual Enabled: true
+    - Class: moveit_rviz_plugin/PlanningScene
+      Enabled: true
+      Move Group Namespace: ""
+      Name: PlanningScene
+      Planning Scene Topic: /moveit_grasps_demo/grasping_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.20000000298023224
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool_center_point:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+    - Class: rviz_visual_tools/KeyTool
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 2.91066575050354
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.6259773373603821
+        Y: 0.3534509539604187
+        Z: 0.4344802796840668
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.14539791643619537
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.575382709503174
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 845
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000164000002acfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073020000042c000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ac000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000220000001c00000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100ffffff000000010000010f000003f1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000003f1000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005fd00000041fc0100000003fb00000024005200760069007a00560069007300750061006c0054006f006f006c00730047007500690100000000000005fd0000016400fffffffb0000000800540069006d0065000000000000000638000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000493000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  RvizVisualToolsGui:
+    collapsed: false
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Trajectory - Trajectory Slider:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1533
+  X: 67
+  Y: 27

mtc_templates/-1406488393.yaml

+planners:
+  - id: cartesian
+    type: CartesianPath
+  - id: sampling
+    type: PipelinePlanner
+    properties:
+      step_size: 0.005
+      goal_joint_tolerance: 1e-05
+  - id: interpolation
+    type: JointInterpolationPlanner
+task:
+  name: Pick and Place test
+  properties:
+    group: Robot_arm1
+    eef: hand_1
+    hand_grasping_frame: panda_1_link8
+    ik_frame: panda_1_link8
+    hand: hand_1
+  stages:
+    - name: current
+      type: CurrentState
+    - name: move to ready
+      type: MoveTo
+      id: ready
+      planner: sampling
+      propertiesConfigureInitFrom:
+        source: PARENT
+        values:
+          - group
+      set:
+        goal: ready
+    - type: MoveTo
+      planner: sampling
+      id: hand_open
+      properties:
+        group: hand_1
+      set:
+        goal: open
+    - type: Connect
+      group_planner_vector:
+        panda_arm: sampling
+      propertiesConfigureInitFrom:
+        source: PARENT
+    - type: SerialContainer
+      name: grasp
+      properties_exposeTo:
+        source: task
+        values: [eef, hand, group, ik_frame]
+      propertiesConfigureInitFrom:
+        source: PARENT
+        values: [eef, hand, group, ik_frame]
+      stages:
+        - type: MoveRelative
+          planner: cartesian
+          properties:
+            link: panda_1_link8
+            min_distance: 0.07
+            max_distance: 0.2
+          propertiesConfigureInitFrom:
+            source: PARENT
+            values: [group]
+          set:
+            direction:
+              vector: {x: 0.0, y: 0.0, z: 1.0, header: {frame_id: panda_1_link8}}
+        - type: ComputeIK
+          properties: {max_ik_solutions: 5}
+          set:
+            ik_frame:
+              isometry:
+                translation: {x: 0.1, y: 0.0, z: 0.0}
+                quaternion: {r: 1.571, p: 0.785, y: 1.571}
+              link: panda_1_link8
+          propertiesConfigureInitFrom:
+            - source: PARENT
+              values: [eef, group]
+            - source: INTERFACE
+              values: [target_pose]
+          stage:
+            type: GenerateGraspPose
+            propertiesConfigureInitFrom:
+              source: PARENT
+            properties:
+              object: bottle
+              angle_delta: 1.571
+              pregrasp: open
+            set:
+              monitored_stage: ready
+        - type: ModifyPlanningScene
+          set:
+            allow_collisions:
+              first: bottle
+              second:
+                joint_model_group_name: hand_1
+              allow: true
+        - type: MoveTo
+          planner: sampling
+          properties:
+            group: hand_1
+          set:
+            goal: close
+        - type: ModifyPlanningScene
+          set:
+            attach_object:
+              object: bottle
+              link: panda_1_link8
+        - type: MoveRelative
+          planner: cartesian
+          id: pick_up
+          propertiesConfigureInitFrom:
+            source: PARENT
+            values: [group]
+          properties:
+            min_distance: 0.1
+            max_distance: 0.2
+          set:
+            ik_frame:
+              link: panda_1_link8
+            direction:
+              vector: {x: 0.0, y: 0.0, z: 1.0}
+    - type: Connect
+      group_planner_vector:
+        panda_arm: sampling
+      propertiesConfigureInitFrom:
+        source: PARENT
+    - type: SerialContainer
+      name: place
+      properties_exposeTo:
+        source: task
+        values: [eef, hand, group, ik_frame]
+      propertiesConfigureInitFrom:
+        source: PARENT
+      stages:
+        - type: MoveRelative
+          planner: cartesian
+          properties:
+            link: panda_1_link8
+            min_distance: 0.1
+            max_distance: 0.2
+          propertiesConfigureInitFrom:
+            source: PARENT
+            values: [group]
+          set:
+            direction:
+              vector: {x: 0.0, y: 0.0, z: -1.0}
+        - type: ComputeIK
+          properties: {max_ik_solutions: 5}
+          set:
+            ik_frame:
+              isometry:
+                translation: {x: 0.1, y: 0.0, z: 0.0}
+                quaternion: {r: 1.571, p: 0.785, y: 1.571}
+              link: panda_1_link8
+          propertiesConfigureInitFrom:
+            - source: PARENT
+              values: [eef, group]
+            - source: INTERFACE
+              values: [target_pose]
+          stage:
+            type: GeneratePose
+            set:
+              monitored_stage: pick_up
+              pose:
+                point: {x: 0.100000, 0.700000: ~, z: 0.9305f}
+                orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
+        - type: MoveTo
+          planner: sampling
+          properties:
+            group: hand_1
+          set:
+            goal: open
+        - type: ModifyPlanningScene
+          set:
+            detach_object:
+              object: bottle
+              link: panda_1_link8
+            allow_collisions:
+              first: bottle
+              second:
+                joint_model_group_name: hand_1
+              allow: false
+        - type: MoveRelative
+          planner: cartesian
+          properties:
+            link: panda_1_link8
+            min_distance: 0.07
+            max_distance: 0.2
+          propertiesConfigureInitFrom:
+            source: PARENT
+            values: [group]
+          set:
+            direction:
+              vector: {x: 0.0, y: 0.0, z: -1.0, header: {frame_id: panda_1_link8}}
+        - type: MoveTo
+          planner: sampling
+          properties:
+            group: hand_1
+          set:
+            goal: close
+        - name: move to ready
+          type: MoveTo
+          planner: sampling
+          propertiesConfigureInitFrom:
+            source: PARENT
+            value:
+              - group
+          set:
+            goal: ready
+max_planning_solutions: 10
\ No newline at end of file

mtc_templates/-854508939.yaml

+planners:
+  - id: cartesian
+    type: CartesianPath
+  - id: sampling
+    type: PipelinePlanner
+    properties:
+      step_size: 0.005
+      goal_joint_tolerance: 1e-05
+  - id: interpolation
+    type: JointInterpolationPlanner
+task:
+  name: Pick and Place test
+  properties:
+    group: Robot_arm1
+    eef: hand_1
+    hand_grasping_frame: panda_1_link8
+    ik_frame: panda_1_link8
+    hand: hand_1
+  stages:
+    - name: current
+      type: CurrentState
+    - name: move to ready
+      type: MoveTo
+      id: ready
+      planner: sampling
+      propertiesConfigureInitFrom:
+        source: PARENT
+        values:
+          - group
+      set:
+        goal: ready
+    - type: MoveTo
+      planner: sampling
+      id: hand_open
+      properties:
+        group: hand_1
+      set:
+        goal: open
+    - type: Connect
+      group_planner_vector:
+        panda_arm: sampling
+      propertiesConfigureInitFrom:
+        source: PARENT
+    - type: SerialContainer
+      name: grasp
+      properties_exposeTo:
+        source: task
+        values: [eef, hand, group, ik_frame]
+      propertiesConfigureInitFrom:
+        source: PARENT
+        values: [eef, hand, group, ik_frame]
+      stages:
+        - type: MoveRelative
+          planner: cartesian
+          properties:
+            link: panda_1_link8
+            min_distance: 0.07
+            max_distance: 0.2
+          propertiesConfigureInitFrom:
+            source: PARENT
+            values: [group]
+          set:
+            direction:
+              vector: {x: 0.0, y: 0.0, z: 1.0, header: {frame_id: panda_1_link8}}
+        - type: ComputeIK
+          properties: {max_ik_solutions: 5}
+          set:
+            ik_frame:
+              isometry:
+                translation: {x: 0.1, y: 0.0, z: 0.0}
+                quaternion: {r: 1.571, p: 0.785, y: 1.571}
+              link: panda_1_link8
+          propertiesConfigureInitFrom:
+            - source: PARENT
+              values: [eef, group]
+            - source: INTERFACE
+              values: [target_pose]
+          stage:
+            type: GenerateGraspPose
+            propertiesConfigureInitFrom:
+              source: PARENT
+            properties:
+              object: bottle
+              angle_delta: 1.571
+              pregrasp: open
+            set:
+              monitored_stage: ready
+        - type: ModifyPlanningScene
+          set:
+            allow_collisions:
+              first: bottle
+              second:
+                joint_model_group_name: hand_1
+              allow: true
+        - type: MoveTo
+          planner: sampling
+          properties:
+            group: hand_1
+          set:
+            goal: close
+        - type: ModifyPlanningScene
+          set:
+            attach_object:
+              object: bottle
+              link: panda_1_link8
+        - type: MoveRelative
+          planner: cartesian
+          id: pick_up
+          propertiesConfigureInitFrom:
+            source: PARENT
+            values: [group]
+          properties:
+            min_distance: 0.1
+            max_distance: 0.2
+          set:
+            ik_frame:
+              link: panda_1_link8
+            direction:
+              vector: {x: 0.0, y: 0.0, z: 1.0}
+    - type: Connect
+      group_planner_vector:
+        panda_arm: sampling
+      propertiesConfigureInitFrom:
+        source: PARENT
+    - type: SerialContainer
+      name: place
+      properties_exposeTo:
+        source: task
+        values: [eef, hand, group, ik_frame]
+      propertiesConfigureInitFrom:
+        source: PARENT
+      stages:
+        - type: MoveRelative
+          planner: cartesian
+          properties:
+            link: panda_1_link8
+            min_distance: 0.1
+            max_distance: 0.2
+          propertiesConfigureInitFrom:
+            source: PARENT
+            values: [group]
+          set:
+            direction:
+              vector: {x: 0.0, y: 0.0, z: -1.0}
+        - type: ComputeIK
+          properties: {max_ik_solutions: 5}
+          set:
+            ik_frame:
+              isometry:
+                translation: {x: 0.1, y: 0.0, z: 0.0}
+                quaternion: {r: 1.571, p: 0.785, y: 1.571}
+              link: panda_1_link8
+          propertiesConfigureInitFrom:
+            - source: PARENT
+              values: [eef, group]
+            - source: INTERFACE
+              values: [target_pose]
+          stage:
+            type: GeneratePose
+            set:
+              monitored_stage: pick_up
+              pose:
+                point: {x: 0.100000, 0.700000: ~, z: 0.9305f}
+                orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
+        - type: MoveTo
+          planner: sampling
+          properties:
+            group: hand_1
+          set:
+            goal: open
+        - type: ModifyPlanningScene
+          set:
+            detach_object:
+              object: bottle
+              link: panda_1_link8
+            allow_collisions:
+              first: bottle
+              second:
+                joint_model_group_name: hand_1
+              allow: false
+        - type: MoveRelative
+          planner: cartesian
+          properties:
+            link: panda_1_link8
+            min_distance: 0.07
+            max_distance: 0.2
+          propertiesConfigureInitFrom:
+            source: PARENT
+            values: [group]
+          set:
+            direction:
+              vector: {x: 0.0, y: 0.0, z: -1.0, header: {frame_id: panda_1_link8}}
+        - type: MoveTo
+          planner: sampling
+          properties:
+            group: hand_1
+          set:
+            goal: close
+        - name: move to ready
+          type: MoveTo
+          planner: sampling
+          propertiesConfigureInitFrom:
+            source: PARENT
+            value:
+              - group
+          set:
+            goal: ready
+max_planning_solutions: 10
\ No newline at end of file

mtc_templates/dummy.yaml

+planners:
+  - id: cartesian
+    type: CartesianPath
+  - id: sampling
+    type: PipelinePlanner
+    properties:
+      step_size: 0.005
+      goal_joint_tolerance: 9.999999747378752e-06
+  - id: interpolation
+    type: JointInterpolationPlanner
+task:
+  name: Pick and Place test
+  properties:
+    group: ""
+    eef: ""
+    hand_grasping_frame: ""
+    ik_frame: ""
+    hand: ""
+  stages:
+    - name: current
+      type: CurrentState
+    - name: move to ready
+      type: MoveTo
+      id: ready
+      planner: sampling
+      propertiesConfigureInitFrom:
+        source: PARENT
+        values:
+          - group
+      set:
+        goal: ready
+    - type: MoveTo
+      planner: sampling
+      id: hand_open
+      properties:
+        group: ""
+      set:
+        goal: open
+    - type: Connect
+      group_planner_vector:
+        panda_arm1: sampling
+      propertiesConfigureInitFrom:
+        source: PARENT
+    - type: SerialContainer
+      name: grasp
+      properties_exposeTo:
+        source: task
+        values: [eef, hand, group, ik_frame]
+      propertiesConfigureInitFrom:
+        source: PARENT
+        values: [eef, hand, group, ik_frame]
+      stages:
+        - type: MoveRelative
+          planner: cartesian
+          properties:
+            link: ""
+            min_distance: 0
+            max_distance: 0
+          propertiesConfigureInitFrom:
+            source: PARENT
+            values: [group]
+          set:
+            direction:
+              vector: {x: 0.0, y: 0.0, z: 1.0, header: {frame_id: ""}}
+        - type: ComputeIK
+          properties: {max_ik_solutions: 5}
+          set:
+            ik_frame:
+              isometry:
+                translation: {x: 0.1, y: 0.0, z: 0.0}
+                quaternion: {r: 1.571, p: 0.785, y: 1.571}
+              link: ""
+          propertiesConfigureInitFrom:
+            - source: PARENT
+              values: [eef, group]
+            - source: INTERFACE
+              values: [target_pose]
+          stage:
+            type: GenerateGraspPose
+            propertiesConfigureInitFrom:
+              source: PARENT
+            properties:
+              object: ""
+              angle_delta: 1.571
+              pregrasp: open
+            set:
+              monitored_stage: ready
+        - type: ModifyPlanningScene
+          set:
+            allow_collisions:
+              first: ""
+              second:
+                joint_model_group_name: ""
+              allow: true
+        - type: MoveTo
+          planner: sampling
+          properties:
+            group: ""
+          set:
+            goal: close
+        - type: ModifyPlanningScene
+          set:
+            attach_object:
+              object: ""
+              link: ""
+        - type: ModifyPlanningScene
+          set:
+            allow_collisions:
+              first: ""
+              second: ""
+              allow: true
+        - type: MoveRelative
+          planner: cartesian
+          id: pick_up
+          propertiesConfigureInitFrom:
+            source: PARENT
+            values: [group]
+          properties:
+            min_distance: 0.1
+            max_distance: 0.2
+          set:
+            ik_frame:
+              link: ""
+            direction:
+              vector: {x: 0.0, y: 0.0, z: 1.0}
+        - type: ModifyPlanningScene
+          set:
+            allow_collisions:
+              first: ""
+              second: ""
+              allow: false
+    - type: Connect
+      group_planner_vector:
+        panda_arm1: sampling
+      propertiesConfigureInitFrom:
+        source: PARENT
+    - type: SerialContainer
+      name: place
+      properties_exposeTo:
+        source: task
+        values: [eef, hand, group, ik_frame]
+      propertiesConfigureInitFrom:
+        source: PARENT
+      stages:
+        - type: ModifyPlanningScene
+          set:
+            allow_collisions:
+              first: ""
+              second: ""
+              allow: true
+        - type: MoveRelative
+          planner: cartesian
+          properties:
+            link: ""
+            min_distance: 0
+            max_distance: 0
+          propertiesConfigureInitFrom:
+            source: PARENT
+            values: [group]
+          set:
+            direction:
+              vector: {x: 0.0, y: 0.0, z: -1.0}
+        - type: ComputeIK
+          properties: {max_ik_solutions: 5}
+          set:
+            ik_frame:
+              isometry:
+                translation: {x: 0.1, y: 0.0, z: 0.0}
+                quaternion: {r: 1.571, p: 0.785, y: 1.571}
+              link: ""
+          propertiesConfigureInitFrom:
+            - source: PARENT
+              values: [eef, group]
+            - source: INTERFACE
+              values: [target_pose]
+          stage:
+            type: GeneratePose
+            set:
+              monitored_stage: pick_up
+              pose:
+                point: {x: 0, y: 0, z: 0}
+                orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
+        - type: MoveTo
+          planner: sampling
+          properties:
+            group: ""
+          set:
+            goal: open
+        - type: ModifyPlanningScene
+          set:
+            detach_object:
+              object: ""
+              link: ""
+            allow_collisions:
+              first: ""
+              second:
+                joint_model_group_name: ""
+              allow: false
+        - type: MoveRelative
+          planner: cartesian
+          properties:
+            link: ""
+            min_distance: 0
+            max_distance: 0
+          propertiesConfigureInitFrom:
+            source: PARENT
+            values: [group]
+          set:
+            direction:
+              vector: {x: 0.0, y: 0.0, z: -1.0, header: {frame_id: ""}}
+        - type: MoveTo
+          planner: sampling
+          properties:
+            group: ""
+          set:
+            goal: close
+        - name: move to ready
+          type: MoveTo
+          planner: sampling
+          propertiesConfigureInitFrom:
+            source: PARENT
+            values:
+              - group
+          set:
+            goal: ready
+max_planning_solutions: 10