diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index f0fa9d9d117da6367d04dd3636ec84b944d8264c..f49f83e06dbf6d7261ee5ef8fdecfba0d74fbda2 100644
--- a/.catkin_tools/profiles/default/devel_collisions.txt
+++ b/.catkin_tools/profiles/default/devel_collisions.txt
@@ -1,4 +1,4 @@
/home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 11297
+/home/matteo/reachability/devel/./cmake.lock 12194
/home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
diff --git a/.catkin_tools/profiles/default/packages/moveit_task_constructor_capabilities/devel_manifest.txt b/.catkin_tools/profiles/default/packages/moveit_task_constructor_capabilities/devel_manifest.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b901b482219c7697a4a35365c1b62ab8c1c40550
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/moveit_task_constructor_capabilities/devel_manifest.txt
@@ -0,0 +1,6 @@
+moveit_task_constructor/capabilities
+/home/matteo/reachability/devel/.private/moveit_task_constructor_capabilities/cmake.lock /home/matteo/reachability/devel/./cmake.lock
+/home/matteo/reachability/devel/.private/moveit_task_constructor_capabilities/share/moveit_task_constructor_capabilities/cmake/moveit_task_constructor_capabilitiesConfig.cmake /home/matteo/reachability/devel/share/moveit_task_constructor_capabilities/cmake/moveit_task_constructor_capabilitiesConfig.cmake
+/home/matteo/reachability/devel/.private/moveit_task_constructor_capabilities/share/moveit_task_constructor_capabilities/cmake/moveit_task_constructor_capabilitiesConfig-version.cmake /home/matteo/reachability/devel/share/moveit_task_constructor_capabilities/cmake/moveit_task_constructor_capabilitiesConfig-version.cmake
+/home/matteo/reachability/devel/.private/moveit_task_constructor_capabilities/lib/libmoveit_task_constructor_capabilities.so /home/matteo/reachability/devel/lib/libmoveit_task_constructor_capabilities.so
+/home/matteo/reachability/devel/.private/moveit_task_constructor_capabilities/lib/pkgconfig/moveit_task_constructor_capabilities.pc /home/matteo/reachability/devel/lib/pkgconfig/moveit_task_constructor_capabilities.pc
diff --git a/.catkin_tools/profiles/default/packages/moveit_task_constructor_capabilities/package.xml b/.catkin_tools/profiles/default/packages/moveit_task_constructor_capabilities/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..7ae112f2b31eca50e12627f58e8346bf50f4a03a
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/moveit_task_constructor_capabilities/package.xml
@@ -0,0 +1,26 @@
+<package format="2">
+ <name>moveit_task_constructor_capabilities</name>
+ <version>0.0.0</version>
+ <description>
+ MoveGroupCapabilites to interact with MoveIt
+ </description>
+
+ <maintainer email="me@v4hn.de">Michael v4hn Goerner</maintainer>
+ <maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
+
+ <license>BSD</license>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <depend>moveit_core</depend>
+ <depend>moveit_ros_move_group</depend>
+ <depend>actionlib</depend>
+ <depend>pluginlib</depend>
+ <depend>std_msgs</depend>
+ <depend>moveit_task_constructor_core</depend>
+ <depend>moveit_task_constructor_msgs</depend>
+
+ <export>
+ <moveit_ros_move_group plugin="${prefix}/capabilities_plugin_description.xml"/>
+ </export>
+</package>
diff --git a/.catkin_tools/profiles/default/packages/moveit_task_constructor_core/devel_manifest.txt b/.catkin_tools/profiles/default/packages/moveit_task_constructor_core/devel_manifest.txt
new file mode 100644
index 0000000000000000000000000000000000000000..4504addb0a1c53608ce39af49166c52bd8a9f10a
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/moveit_task_constructor_core/devel_manifest.txt
@@ -0,0 +1,12 @@
+moveit_task_constructor/core
+/home/matteo/reachability/devel/.private/moveit_task_constructor_core/cmake.lock /home/matteo/reachability/devel/./cmake.lock
+/home/matteo/reachability/devel/.private/moveit_task_constructor_core/share/moveit_task_constructor_core/cmake/moveit_task_constructor_coreConfig-version.cmake /home/matteo/reachability/devel/share/moveit_task_constructor_core/cmake/moveit_task_constructor_coreConfig-version.cmake
+/home/matteo/reachability/devel/.private/moveit_task_constructor_core/share/moveit_task_constructor_core/cmake/moveit_task_constructor_coreConfig.cmake /home/matteo/reachability/devel/share/moveit_task_constructor_core/cmake/moveit_task_constructor_coreConfig.cmake
+/home/matteo/reachability/devel/.private/moveit_task_constructor_core/lib/libmoveit_task_constructor_core.so /home/matteo/reachability/devel/lib/libmoveit_task_constructor_core.so
+/home/matteo/reachability/devel/.private/moveit_task_constructor_core/lib/libmoveit_task_constructor_core_stage_plugins.so /home/matteo/reachability/devel/lib/libmoveit_task_constructor_core_stage_plugins.so
+/home/matteo/reachability/devel/.private/moveit_task_constructor_core/lib/libgtest_utils.so /home/matteo/reachability/devel/lib/libgtest_utils.so
+/home/matteo/reachability/devel/.private/moveit_task_constructor_core/lib/libmoveit_task_constructor_core_stages.so /home/matteo/reachability/devel/lib/libmoveit_task_constructor_core_stages.so
+/home/matteo/reachability/devel/.private/moveit_task_constructor_core/lib/moveit_task_constructor_core/pick_pr2 /home/matteo/reachability/devel/lib/moveit_task_constructor_core/pick_pr2
+/home/matteo/reachability/devel/.private/moveit_task_constructor_core/lib/moveit_task_constructor_core/pick_pa10 /home/matteo/reachability/devel/lib/moveit_task_constructor_core/pick_pa10
+/home/matteo/reachability/devel/.private/moveit_task_constructor_core/lib/moveit_task_constructor_core/pick_ur5 /home/matteo/reachability/devel/lib/moveit_task_constructor_core/pick_ur5
+/home/matteo/reachability/devel/.private/moveit_task_constructor_core/lib/pkgconfig/moveit_task_constructor_core.pc /home/matteo/reachability/devel/lib/pkgconfig/moveit_task_constructor_core.pc
diff --git a/.catkin_tools/profiles/default/packages/moveit_task_constructor_core/package.xml b/.catkin_tools/profiles/default/packages/moveit_task_constructor_core/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..8fbadef252ad5ce9ee6accb38ccc0c29f7027cad
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/moveit_task_constructor_core/package.xml
@@ -0,0 +1,31 @@
+<package format="2">
+ <name>moveit_task_constructor_core</name>
+ <version>0.0.0</version>
+ <description>MoveIt Task Pipeline</description>
+
+ <license>BSD</license>
+ <maintainer email="me@v4hn.de">Michael Goerner</maintainer>
+ <maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>roscpp</build_depend>
+ <exec_depend>roscpp</exec_depend>
+
+ <depend>tf2_eigen</depend>
+ <depend>geometry_msgs</depend>
+ <depend>moveit_core</depend>
+ <depend>moveit_ros_planning</depend>
+ <depend>moveit_ros_planning_interface</depend>
+ <depend>moveit_task_constructor_msgs</depend>
+ <depend>visualization_msgs</depend>
+ <depend>rviz_marker_tools</depend>
+
+ <test_depend>rosunit</test_depend>
+ <test_depend>rostest</test_depend>
+ <test_depend>moveit_resources_fanuc_moveit_config</test_depend>
+
+ <export>
+ <moveit_task_constructor_core plugin="${prefix}/motion_planning_stages_plugin_description.xml"/>
+ </export>
+</package>
diff --git a/.catkin_tools/profiles/default/packages/moveit_task_constructor_demo/devel_manifest.txt b/.catkin_tools/profiles/default/packages/moveit_task_constructor_demo/devel_manifest.txt
new file mode 100644
index 0000000000000000000000000000000000000000..429b067b347c823b4d0539f8814e8046c856705a
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/moveit_task_constructor_demo/devel_manifest.txt
@@ -0,0 +1,12 @@
+moveit_task_constructor/demo
+/home/matteo/reachability/devel/.private/moveit_task_constructor_demo/cmake.lock /home/matteo/reachability/devel/./cmake.lock
+/home/matteo/reachability/devel/.private/moveit_task_constructor_demo/share/moveit_task_constructor_demo/cmake/moveit_task_constructor_demoConfig-version.cmake /home/matteo/reachability/devel/share/moveit_task_constructor_demo/cmake/moveit_task_constructor_demoConfig-version.cmake
+/home/matteo/reachability/devel/.private/moveit_task_constructor_demo/share/moveit_task_constructor_demo/cmake/moveit_task_constructor_demoConfig.cmake /home/matteo/reachability/devel/share/moveit_task_constructor_demo/cmake/moveit_task_constructor_demoConfig.cmake
+/home/matteo/reachability/devel/.private/moveit_task_constructor_demo/lib/libmoveit_task_constructor_demo_pick_place_task.so /home/matteo/reachability/devel/lib/libmoveit_task_constructor_demo_pick_place_task.so
+/home/matteo/reachability/devel/.private/moveit_task_constructor_demo/lib/pkgconfig/moveit_task_constructor_demo.pc /home/matteo/reachability/devel/lib/pkgconfig/moveit_task_constructor_demo.pc
+/home/matteo/reachability/devel/.private/moveit_task_constructor_demo/lib/moveit_task_constructor_demo/pick_place_demo /home/matteo/reachability/devel/lib/moveit_task_constructor_demo/pick_place_demo
+/home/matteo/reachability/devel/.private/moveit_task_constructor_demo/lib/moveit_task_constructor_demo/cartesian /home/matteo/reachability/devel/lib/moveit_task_constructor_demo/cartesian
+/home/matteo/reachability/devel/.private/moveit_task_constructor_demo/lib/moveit_task_constructor_demo/fallbacks_move_to /home/matteo/reachability/devel/lib/moveit_task_constructor_demo/fallbacks_move_to
+/home/matteo/reachability/devel/.private/moveit_task_constructor_demo/lib/moveit_task_constructor_demo/modular /home/matteo/reachability/devel/lib/moveit_task_constructor_demo/modular
+/home/matteo/reachability/devel/.private/moveit_task_constructor_demo/lib/moveit_task_constructor_demo/ik_clearance_cost /home/matteo/reachability/devel/lib/moveit_task_constructor_demo/ik_clearance_cost
+/home/matteo/reachability/devel/.private/moveit_task_constructor_demo/lib/moveit_task_constructor_demo/alternative_path_costs /home/matteo/reachability/devel/lib/moveit_task_constructor_demo/alternative_path_costs
diff --git a/.catkin_tools/profiles/default/packages/moveit_task_constructor_demo/package.xml b/.catkin_tools/profiles/default/packages/moveit_task_constructor_demo/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..c4ccda3dc7ded3e7ace4d523644ad625535c0446
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/moveit_task_constructor_demo/package.xml
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>moveit_task_constructor_demo</name>
+ <version>0.0.1</version>
+ <description>demo tasks illustrating various capabilities of MTC.</description>
+
+ <author email="simongold@picknik.ai">simon Goldstein</author>
+ <author email="henningkayser@picknik.ai">Henning Kayser</author>
+ <maintainer email="henningkayser@picknik.ai">Henning Kayser</maintainer>
+
+ <license>BSD</license>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <depend>roscpp</depend>
+ <depend>rosparam_shortcuts</depend>
+
+ <depend>moveit_core</depend>
+ <depend>moveit_ros_planning_interface</depend>
+
+ <depend>moveit_task_constructor_core</depend>
+
+ <exec_depend>moveit_task_constructor_capabilities</exec_depend>
+ <exec_depend>moveit_resources_panda_moveit_config</exec_depend>
+
+ <test_depend>rostest</test_depend>
+</package>
diff --git a/.catkin_tools/profiles/default/packages/moveit_task_constructor_msgs/devel_manifest.txt b/.catkin_tools/profiles/default/packages/moveit_task_constructor_msgs/devel_manifest.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b99af099c89a4d80d4da4414ae9bbac5fca3129d
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/moveit_task_constructor_msgs/devel_manifest.txt
@@ -0,0 +1,129 @@
+moveit_task_constructor/msgs
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/cmake.lock /home/matteo/reachability/devel/./cmake.lock
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/srv/moveit_task_constructor_msgs-srv.asd /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/srv/moveit_task_constructor_msgs-srv.asd
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/srv/_package_GetSolution.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/srv/_package_GetSolution.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/srv/GetSolution.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/srv/GetSolution.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/srv/_package.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/srv/_package.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_SubTrajectory.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_SubTrajectory.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_SolutionInfo.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_SolutionInfo.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_Solution.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_Solution.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionFeedback.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionFeedback.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/SubTrajectory.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/SubTrajectory.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionResult.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionResult.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_ExecuteTaskSolutionResult.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_ExecuteTaskSolutionResult.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionGoal.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionGoal.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionFeedback.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionFeedback.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/TaskDescription.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/TaskDescription.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_ExecuteTaskSolutionActionResult.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_ExecuteTaskSolutionActionResult.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/Solution.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/Solution.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionResult.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionResult.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/SubSolution.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/SubSolution.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_TaskDescription.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_TaskDescription.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_StageStatistics.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_StageStatistics.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/StageStatistics.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/StageStatistics.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_StageDescription.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_StageDescription.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/moveit_task_constructor_msgs-msg.asd /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/moveit_task_constructor_msgs-msg.asd
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/TaskStatistics.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/TaskStatistics.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_ExecuteTaskSolutionFeedback.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_ExecuteTaskSolutionFeedback.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_TaskStatistics.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_TaskStatistics.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionGoal.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionGoal.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_ExecuteTaskSolutionAction.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_ExecuteTaskSolutionAction.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_ExecuteTaskSolutionActionGoal.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_ExecuteTaskSolutionActionGoal.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/Property.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/Property.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionAction.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionAction.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_ExecuteTaskSolutionGoal.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_ExecuteTaskSolutionGoal.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/StageDescription.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/StageDescription.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_Property.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_Property.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/SolutionInfo.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/SolutionInfo.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_ExecuteTaskSolutionActionFeedback.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_ExecuteTaskSolutionActionFeedback.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_SubSolution.lisp /home/matteo/reachability/devel/share/common-lisp/ros/moveit_task_constructor_msgs/msg/_package_SubSolution.lisp
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/manifest.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/manifest.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/srv/GetSolution.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/srv/GetSolution.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/TaskStatistics.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/TaskStatistics.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/TaskDescription.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/TaskDescription.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/StageStatistics.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/StageStatistics.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionAction.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionAction.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionFeedback.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionFeedback.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/SolutionInfo.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/SolutionInfo.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionGoal.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionGoal.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/Solution.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/Solution.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/Property.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/Property.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/StageDescription.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/StageDescription.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionGoal.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionGoal.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionFeedback.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionFeedback.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionResult.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionResult.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/SubTrajectory.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/SubTrajectory.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionResult.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionResult.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/roseus/ros/moveit_task_constructor_msgs/msg/SubSolution.l /home/matteo/reachability/devel/share/roseus/ros/moveit_task_constructor_msgs/msg/SubSolution.l
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/moveit_task_constructor_msgs/cmake/moveit_task_constructor_msgsConfig.cmake /home/matteo/reachability/devel/share/moveit_task_constructor_msgs/cmake/moveit_task_constructor_msgsConfig.cmake
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/moveit_task_constructor_msgs/cmake/moveit_task_constructor_msgsConfig-version.cmake /home/matteo/reachability/devel/share/moveit_task_constructor_msgs/cmake/moveit_task_constructor_msgsConfig-version.cmake
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/moveit_task_constructor_msgs/cmake/moveit_task_constructor_msgs-msg-extras.cmake /home/matteo/reachability/devel/share/moveit_task_constructor_msgs/cmake/moveit_task_constructor_msgs-msg-extras.cmake
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/moveit_task_constructor_msgs/cmake/moveit_task_constructor_msgs-msg-paths.cmake /home/matteo/reachability/devel/share/moveit_task_constructor_msgs/cmake/moveit_task_constructor_msgs-msg-paths.cmake
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionGoal.msg /home/matteo/reachability/devel/share/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionGoal.msg
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionAction.msg /home/matteo/reachability/devel/share/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionAction.msg
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionGoal.msg /home/matteo/reachability/devel/share/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionGoal.msg
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionFeedback.msg /home/matteo/reachability/devel/share/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionFeedback.msg
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionFeedback.msg /home/matteo/reachability/devel/share/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionFeedback.msg
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionResult.msg /home/matteo/reachability/devel/share/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionResult.msg
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionResult.msg /home/matteo/reachability/devel/share/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionResult.msg
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/_index.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/_index.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/srv/GetSolution.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/srv/GetSolution.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/srv/_index.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/srv/_index.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/TaskDescription.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/TaskDescription.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/SubSolution.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/SubSolution.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/SubTrajectory.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/SubTrajectory.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/StageStatistics.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/StageStatistics.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionResult.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionResult.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionFeedback.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionFeedback.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionGoal.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionGoal.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionGoal.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionActionGoal.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/_index.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/_index.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/Property.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/Property.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/StageDescription.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/StageDescription.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionResult.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionResult.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/SolutionInfo.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/SolutionInfo.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionAction.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionAction.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/TaskStatistics.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/TaskStatistics.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/Solution.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/Solution.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/share/gennodejs/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionFeedback.js /home/matteo/reachability/devel/share/gennodejs/ros/moveit_task_constructor_msgs/msg/ExecuteTaskSolutionFeedback.js
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/TaskStatistics.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/TaskStatistics.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/SolutionInfo.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/SolutionInfo.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/StageStatistics.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/StageStatistics.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/TaskDescription.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/TaskDescription.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/ExecuteTaskSolutionActionResult.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/ExecuteTaskSolutionActionResult.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/ExecuteTaskSolutionActionGoal.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/ExecuteTaskSolutionActionGoal.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/ExecuteTaskSolutionActionFeedback.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/ExecuteTaskSolutionActionFeedback.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/ExecuteTaskSolutionFeedback.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/ExecuteTaskSolutionFeedback.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/Property.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/Property.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/GetSolutionRequest.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/GetSolutionRequest.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/StageDescription.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/StageDescription.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/SubTrajectory.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/SubTrajectory.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/GetSolutionResponse.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/GetSolutionResponse.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/SubSolution.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/SubSolution.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/ExecuteTaskSolutionResult.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/ExecuteTaskSolutionResult.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/ExecuteTaskSolutionGoal.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/ExecuteTaskSolutionGoal.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/GetSolution.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/GetSolution.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/include/moveit_task_constructor_msgs/Solution.h /home/matteo/reachability/devel/include/moveit_task_constructor_msgs/Solution.h
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/pkgconfig/moveit_task_constructor_msgs.pc /home/matteo/reachability/devel/lib/pkgconfig/moveit_task_constructor_msgs.pc
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/__init__.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/__init__.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/srv/__init__.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/srv/__init__.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/srv/_GetSolution.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/srv/_GetSolution.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_ExecuteTaskSolutionActionResult.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_ExecuteTaskSolutionActionResult.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/__init__.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/__init__.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_ExecuteTaskSolutionResult.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_ExecuteTaskSolutionResult.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_ExecuteTaskSolutionActionFeedback.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_ExecuteTaskSolutionActionFeedback.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_ExecuteTaskSolutionAction.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_ExecuteTaskSolutionAction.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_SolutionInfo.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_SolutionInfo.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_ExecuteTaskSolutionFeedback.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_ExecuteTaskSolutionFeedback.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_StageDescription.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_StageDescription.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_Property.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_Property.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_SubSolution.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_SubSolution.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_ExecuteTaskSolutionGoal.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_ExecuteTaskSolutionGoal.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_SubTrajectory.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_SubTrajectory.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_StageStatistics.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_StageStatistics.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_TaskDescription.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_TaskDescription.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_Solution.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_Solution.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_TaskStatistics.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_TaskStatistics.py
+/home/matteo/reachability/devel/.private/moveit_task_constructor_msgs/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_ExecuteTaskSolutionActionGoal.py /home/matteo/reachability/devel/lib/python3/dist-packages/moveit_task_constructor_msgs/msg/_ExecuteTaskSolutionActionGoal.py
diff --git a/.catkin_tools/profiles/default/packages/moveit_task_constructor_msgs/package.xml b/.catkin_tools/profiles/default/packages/moveit_task_constructor_msgs/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..f0bf4ec576d0aea3a93a290f022a24513720bc89
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/moveit_task_constructor_msgs/package.xml
@@ -0,0 +1,17 @@
+<package format="2">
+ <name>moveit_task_constructor_msgs</name>
+ <version>0.0.0</version>
+ <description>Messages for MoveIt Task Pipeline</description>
+
+ <license>BSD</license>
+ <maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
+ <maintainer email="me@v4hn.de">Michael Goerner</maintainer>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <depend>message_generation</depend>
+ <depend>moveit_msgs</depend>
+ <depend>visualization_msgs</depend>
+
+ <exec_depend>message_runtime</exec_depend>
+</package>
diff --git a/.catkin_tools/profiles/default/packages/moveit_task_constructor_visualization/devel_manifest.txt b/.catkin_tools/profiles/default/packages/moveit_task_constructor_visualization/devel_manifest.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a9749bfc8ac787c3f88a310ce298f18d3f284127
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/moveit_task_constructor_visualization/devel_manifest.txt
@@ -0,0 +1,11 @@
+moveit_task_constructor/visualization
+/home/matteo/reachability/devel/.private/moveit_task_constructor_visualization/cmake.lock /home/matteo/reachability/devel/./cmake.lock
+/home/matteo/reachability/devel/.private/moveit_task_constructor_visualization/share/moveit_task_constructor_visualization/cmake/moveit_task_constructor_visualizationConfig.cmake /home/matteo/reachability/devel/share/moveit_task_constructor_visualization/cmake/moveit_task_constructor_visualizationConfig.cmake
+/home/matteo/reachability/devel/.private/moveit_task_constructor_visualization/share/moveit_task_constructor_visualization/cmake/moveit_task_constructor_visualizationConfig-version.cmake /home/matteo/reachability/devel/share/moveit_task_constructor_visualization/cmake/moveit_task_constructor_visualizationConfig-version.cmake
+/home/matteo/reachability/devel/.private/moveit_task_constructor_visualization/lib/libmoveit_task_visualization_tools.so /home/matteo/reachability/devel/lib/libmoveit_task_visualization_tools.so
+/home/matteo/reachability/devel/.private/moveit_task_constructor_visualization/lib/libmoveit_task_visualization_tools.so.0.0.0 /home/matteo/reachability/devel/lib/libmoveit_task_visualization_tools.so.0.0.0
+/home/matteo/reachability/devel/.private/moveit_task_constructor_visualization/lib/libmotion_planning_tasks_rviz_plugin.so.0.0.0 /home/matteo/reachability/devel/lib/libmotion_planning_tasks_rviz_plugin.so.0.0.0
+/home/matteo/reachability/devel/.private/moveit_task_constructor_visualization/lib/libmotion_planning_tasks_properties.so /home/matteo/reachability/devel/lib/libmotion_planning_tasks_properties.so
+/home/matteo/reachability/devel/.private/moveit_task_constructor_visualization/lib/libmotion_planning_tasks_rviz_plugin.so /home/matteo/reachability/devel/lib/libmotion_planning_tasks_rviz_plugin.so
+/home/matteo/reachability/devel/.private/moveit_task_constructor_visualization/lib/libmotion_planning_tasks_utils.so /home/matteo/reachability/devel/lib/libmotion_planning_tasks_utils.so
+/home/matteo/reachability/devel/.private/moveit_task_constructor_visualization/lib/pkgconfig/moveit_task_constructor_visualization.pc /home/matteo/reachability/devel/lib/pkgconfig/moveit_task_constructor_visualization.pc
diff --git a/.catkin_tools/profiles/default/packages/moveit_task_constructor_visualization/package.xml b/.catkin_tools/profiles/default/packages/moveit_task_constructor_visualization/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..196503bf6eb8496bf612a0762b382b958ecc0830
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/moveit_task_constructor_visualization/package.xml
@@ -0,0 +1,27 @@
+<package format="2">
+ <name>moveit_task_constructor_visualization</name>
+ <version>0.0.0</version>
+ <description>Visualization tools for MoveIt Task Pipeline</description>
+
+ <license>BSD</license>
+ <maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
+ <maintainer email="me@v4hn.de">Michael Goerner</maintainer>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>qtbase5-dev</build_depend>
+ <depend>moveit_core</depend>
+ <depend>moveit_task_constructor_msgs</depend>
+ <depend>moveit_task_constructor_core</depend>
+ <depend>moveit_ros_visualization</depend>
+ <depend>roscpp</depend>
+ <depend>rviz</depend>
+
+ <test_depend>rosunit</test_depend>
+ <test_depend>rostest</test_depend>
+ <test_depend>moveit_resources_fanuc_moveit_config</test_depend>
+
+ <export>
+ <rviz plugin="${prefix}/motion_planning_tasks_rviz_plugin_description.xml"/>
+ </export>
+</package>
diff --git a/.catkin_tools/profiles/default/packages/mtc/package.xml b/.catkin_tools/profiles/default/packages/mtc/package.xml
index bea7c8f85b4e24a9ecbac5110921c1f8a24fed18..cd7cd58d6af1f909d2e16230737f9a0386f8c3b8 100644
--- a/.catkin_tools/profiles/default/packages/mtc/package.xml
+++ b/.catkin_tools/profiles/default/packages/mtc/package.xml
@@ -36,6 +36,7 @@
<build_depend>geometry_msgs</build_depend>
<build_depend>YAML_CPP</build_depend>
<build_depend>pcl_ros</build_depend>
+ <build_depend>yaml_to_mtc</build_depend>
diff --git a/.catkin_tools/profiles/default/packages/object_creator/devel_manifest.txt b/.catkin_tools/profiles/default/packages/object_creator/devel_manifest.txt
new file mode 100644
index 0000000000000000000000000000000000000000..95771f85dbc3b66eba39f126256debc1f0b91dc4
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/object_creator/devel_manifest.txt
@@ -0,0 +1,6 @@
+object_creator
+/home/matteo/reachability/devel/.private/object_creator/cmake.lock /home/matteo/reachability/devel/./cmake.lock
+/home/matteo/reachability/devel/.private/object_creator/share/object_creator/cmake/object_creatorConfig-version.cmake /home/matteo/reachability/devel/share/object_creator/cmake/object_creatorConfig-version.cmake
+/home/matteo/reachability/devel/.private/object_creator/share/object_creator/cmake/object_creatorConfig.cmake /home/matteo/reachability/devel/share/object_creator/cmake/object_creatorConfig.cmake
+/home/matteo/reachability/devel/.private/object_creator/lib/libObjectCreator.so /home/matteo/reachability/devel/lib/libObjectCreator.so
+/home/matteo/reachability/devel/.private/object_creator/lib/pkgconfig/object_creator.pc /home/matteo/reachability/devel/lib/pkgconfig/object_creator.pc
diff --git a/.catkin_tools/profiles/default/packages/object_creator/package.xml b/.catkin_tools/profiles/default/packages/object_creator/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..f948dd4187a24a2aa4fb301ed87c04d80dc94bcd
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/object_creator/package.xml
@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>object_creator</name>
+ <version>0.0.0</version>
+ <description>The object_creator package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <maintainer email="jim.molkenthin@mailbox.tu-dresden.de">Jim Molkenthin</maintainer>
+
+
+ <license>BSD</license>
+
+
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_export_depend>roscpp</build_export_depend>
+ <build_export_depend>std_msgs</build_export_depend>
+ <exec_depend>roscpp</exec_depend>
+ <exec_depend>std_msgs</exec_depend>
+
+ <depend>simulation_util</depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/.catkin_tools/profiles/default/packages/panda_simulation/devel_manifest.txt b/.catkin_tools/profiles/default/packages/panda_simulation/devel_manifest.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8d83e475f60771c28a88cbf6083c9f22911d03b6
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/panda_simulation/devel_manifest.txt
@@ -0,0 +1,7 @@
+panda_simulation
+/home/matteo/reachability/devel/.private/panda_simulation/cmake.lock /home/matteo/reachability/devel/./cmake.lock
+/home/matteo/reachability/devel/.private/panda_simulation/share/panda_simulation/cmake/panda_simulationConfig-version.cmake /home/matteo/reachability/devel/share/panda_simulation/cmake/panda_simulationConfig-version.cmake
+/home/matteo/reachability/devel/.private/panda_simulation/share/panda_simulation/cmake/panda_simulationConfig.cmake /home/matteo/reachability/devel/share/panda_simulation/cmake/panda_simulationConfig.cmake
+/home/matteo/reachability/devel/.private/panda_simulation/lib/libpanda_simulation_controllers_lib.so /home/matteo/reachability/devel/lib/libpanda_simulation_controllers_lib.so
+/home/matteo/reachability/devel/.private/panda_simulation/lib/panda_simulation/robot_state_initializer_node /home/matteo/reachability/devel/lib/panda_simulation/robot_state_initializer_node
+/home/matteo/reachability/devel/.private/panda_simulation/lib/pkgconfig/panda_simulation.pc /home/matteo/reachability/devel/lib/pkgconfig/panda_simulation.pc
diff --git a/.catkin_tools/profiles/default/packages/panda_simulation/package.xml b/.catkin_tools/profiles/default/packages/panda_simulation/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0ffcc3c252aa6c25958af41f7e89e5f3f6d1f4f1
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/panda_simulation/package.xml
@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>panda_simulation</name>
+ <version>0.2.0</version>
+ <description>The panda_simulation package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="johannes.mey@tu-dresden.de">Johannes Mey</maintainer>
+ <maintainer email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</maintainer>
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <url type="website">http://wiki.ros.org/gazebo_zones</url>
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="johannes.mey@tu-dresden.de">Johannes Mey</author>
+ <author email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</author>
+ <author>Erdal Pekel</author>
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>MIT</license>
+
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <depend>controller_manager</depend>
+ <depend>effort_controllers</depend>
+<!-- <depend>gazebo_ros</depend>-->
+ <depend>gazebo_plugins</depend>
+ <depend>gazebo_ros_control</depend>
+ <depend>joint_state_controller</depend>
+ <depend>joint_state_publisher</depend>
+ <depend>joint_trajectory_controller</depend>
+ <depend>robot_state_publisher</depend>
+ <depend>roscpp</depend>
+ <depend>std_msgs</depend>
+ <depend>tf</depend>
+ <depend>xacro</depend>
+ <depend>moveit_simple_controller_manager</depend>
+
+ <!-- CUSTOM -->
+ <depend>moveit_core</depend>
+ <depend>moveit_ros_planning_interface</depend>
+ <depend>moveit_ros_perception</depend>
+ <depend>moveit_visual_tools</depend>
+ <depend>controller_interface</depend>
+ <depend>hardware_interface</depend>
+
+
+ <build_depend>sdformat</build_depend>
+ <exec_depend>sdformat</exec_depend>
+
+ <exec_depend>gazebo_ros</exec_depend>
+
+ <exec_depend>pluginlib</exec_depend>
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <controller_interface plugin="${prefix}/controller_plugins.xml" />
+ <gazebo_ros gazebo_media_path="/usr/share/gazebo-9/media/materials/scripts"/>
+ </export>
+</package>
diff --git a/.catkin_tools/profiles/default/packages/rviz_marker_tools/devel_manifest.txt b/.catkin_tools/profiles/default/packages/rviz_marker_tools/devel_manifest.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0edc248ae2b8aed9ef71337e98f91765d1f97857
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/rviz_marker_tools/devel_manifest.txt
@@ -0,0 +1,6 @@
+moveit_task_constructor/rviz_marker_tools
+/home/matteo/reachability/devel/.private/rviz_marker_tools/cmake.lock /home/matteo/reachability/devel/./cmake.lock
+/home/matteo/reachability/devel/.private/rviz_marker_tools/share/rviz_marker_tools/cmake/rviz_marker_toolsConfig.cmake /home/matteo/reachability/devel/share/rviz_marker_tools/cmake/rviz_marker_toolsConfig.cmake
+/home/matteo/reachability/devel/.private/rviz_marker_tools/share/rviz_marker_tools/cmake/rviz_marker_toolsConfig-version.cmake /home/matteo/reachability/devel/share/rviz_marker_tools/cmake/rviz_marker_toolsConfig-version.cmake
+/home/matteo/reachability/devel/.private/rviz_marker_tools/lib/librviz_marker_tools.so /home/matteo/reachability/devel/lib/librviz_marker_tools.so
+/home/matteo/reachability/devel/.private/rviz_marker_tools/lib/pkgconfig/rviz_marker_tools.pc /home/matteo/reachability/devel/lib/pkgconfig/rviz_marker_tools.pc
diff --git a/.catkin_tools/profiles/default/packages/rviz_marker_tools/package.xml b/.catkin_tools/profiles/default/packages/rviz_marker_tools/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..17b0dc408c7a6aeafbc6af46f13862ea19469884
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/rviz_marker_tools/package.xml
@@ -0,0 +1,15 @@
+<package format="2">
+ <name>rviz_marker_tools</name>
+ <version>0.0.0</version>
+ <description>Tools for marker creation / handling</description>
+
+ <license>BSD</license>
+ <maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <depend>visualization_msgs</depend>
+ <depend>geometry_msgs</depend>
+ <depend>roscpp</depend>
+ <depend>rviz</depend>
+</package>
diff --git a/.catkin_tools/profiles/default/packages/simulation_util/devel_manifest.txt b/.catkin_tools/profiles/default/packages/simulation_util/devel_manifest.txt
new file mode 100644
index 0000000000000000000000000000000000000000..21eaaa25093aead9cd549bc245b97d68a2c1c4ce
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/simulation_util/devel_manifest.txt
@@ -0,0 +1,6 @@
+simulation_util
+/home/matteo/reachability/devel/.private/simulation_util/cmake.lock /home/matteo/reachability/devel/./cmake.lock
+/home/matteo/reachability/devel/.private/simulation_util/share/simulation_util/cmake/simulation_utilConfig.cmake /home/matteo/reachability/devel/share/simulation_util/cmake/simulation_utilConfig.cmake
+/home/matteo/reachability/devel/.private/simulation_util/share/simulation_util/cmake/simulation_utilConfig-version.cmake /home/matteo/reachability/devel/share/simulation_util/cmake/simulation_utilConfig-version.cmake
+/home/matteo/reachability/devel/.private/simulation_util/lib/libgazebo_zone_utility.so /home/matteo/reachability/devel/lib/libgazebo_zone_utility.so
+/home/matteo/reachability/devel/.private/simulation_util/lib/pkgconfig/simulation_util.pc /home/matteo/reachability/devel/lib/pkgconfig/simulation_util.pc
diff --git a/.catkin_tools/profiles/default/packages/simulation_util/package.xml b/.catkin_tools/profiles/default/packages/simulation_util/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..acb421b9fd1997e80d41756041f274aef9f9ea2d
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/simulation_util/package.xml
@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>simulation_util</name>
+ <version>0.2.0</version>
+ <description>The simulation_util package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</maintainer>
+ <maintainer email="johannes.mey@tu-dresden.de">Johannes Mey</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>BSD</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <url type="website">http://ceti.pages.st.inf.tu-dresden.de/robotics/pkgs/simulation_util.html</url>
+ <url type="repository">https://git-st.inf.tu-dresden.de/ceti/ros/simulation_util</url>
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <author email="johannes.mey@tu-dresden.de">Johannes Mey</author>
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <depend>roscpp</depend>
+ <depend>sdformat</depend>
+ <build_depend>gazebo_msgs</build_depend>
+ <exec_depend>gazebo_ros</exec_depend>
+ <doc_depend>doxygen</doc_depend>
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/.catkin_tools/profiles/default/packages/yaml_to_mtc/devel_manifest.txt b/.catkin_tools/profiles/default/packages/yaml_to_mtc/devel_manifest.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3883134708e0d570b4db0872ffadaf4c4339c5fb
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/yaml_to_mtc/devel_manifest.txt
@@ -0,0 +1,8 @@
+yaml_to_mtc
+/home/matteo/reachability/devel/.private/yaml_to_mtc/cmake.lock /home/matteo/reachability/devel/./cmake.lock
+/home/matteo/reachability/devel/.private/yaml_to_mtc/share/yaml_to_mtc/cmake/yaml_to_mtcConfig.cmake /home/matteo/reachability/devel/share/yaml_to_mtc/cmake/yaml_to_mtcConfig.cmake
+/home/matteo/reachability/devel/.private/yaml_to_mtc/share/yaml_to_mtc/cmake/yaml_to_mtcConfig-version.cmake /home/matteo/reachability/devel/share/yaml_to_mtc/cmake/yaml_to_mtcConfig-version.cmake
+/home/matteo/reachability/devel/.private/yaml_to_mtc/lib/libyaml_to_mtc.so /home/matteo/reachability/devel/lib/libyaml_to_mtc.so
+/home/matteo/reachability/devel/.private/yaml_to_mtc/lib/yaml_to_mtc/cartesian /home/matteo/reachability/devel/lib/yaml_to_mtc/cartesian
+/home/matteo/reachability/devel/.private/yaml_to_mtc/lib/yaml_to_mtc/pick_pour_place /home/matteo/reachability/devel/lib/yaml_to_mtc/pick_pour_place
+/home/matteo/reachability/devel/.private/yaml_to_mtc/lib/pkgconfig/yaml_to_mtc.pc /home/matteo/reachability/devel/lib/pkgconfig/yaml_to_mtc.pc
diff --git a/.catkin_tools/profiles/default/packages/yaml_to_mtc/package.xml b/.catkin_tools/profiles/default/packages/yaml_to_mtc/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..733f810bdedfb053e58e88f97f3e645d48fb1aba
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/yaml_to_mtc/package.xml
@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>yaml_to_mtc</name>
+ <version>0.0.0</version>
+ <description>The yaml_to_mtc package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="christoph.schroeter1@mailbox.tu-dresden.de">Christoph Schröter</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>BSD</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/yaml_to_mtc</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <author email="christoph.schroeter1@mailbox.tu-dresden.de">Christoph Schröter</author>
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <depend>roscpp</depend>
+ <depend>moveit_task_constructor_core</depend>
+ <depend>simulation_util</depend>
+ <build_depend>object_creator</build_depend>
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>moveit_task_constructor_visualization</build_depend>
+ <exec_depend>moveit_task_constructor_capabilities</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/src/.vscode/c_cpp_properties.json b/src/.vscode/c_cpp_properties.json
index f67badc18cf5ae6175e1331b920da00ffe2a1159..ce67cd6a4995b1fbd3375c6d7dd0b54d7663da32 100644
--- a/src/.vscode/c_cpp_properties.json
+++ b/src/.vscode/c_cpp_properties.json
@@ -35,7 +35,10 @@
"/usr/include/**",
"${workspaceFolder}/mtc/include",
"${workspaceFolder}/mtc/include/impl",
- "${workspaceFolder}/mtc/include/impl/ho"
+ "${workspaceFolder}/mtc/include/impl/ho",
+ "${workspaceFolder}/mtc/src",
+ "${workspaceFolder}/yaml_to_mtc/include",
+ "${workspaceFolder}/moveit_task_constructor/core/include"
],
"name": "ROS"
}
diff --git a/src/ceti_double/config/panda.srdf b/src/ceti_double/config/panda.srdf
index 65e62271ee82b52b7be7922a4f27e75e7b2fd82b..42aab71124d19fc40a189584b30ec3042b5096a9 100644
--- a/src/ceti_double/config/panda.srdf
+++ b/src/ceti_double/config/panda.srdf
@@ -26,27 +26,46 @@
<link name="panda_2_rightfinger"/>
</group>
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
- <group_state name="home1" group="panda_arm1">
+ <group_state name="ready" group="panda_arm1">
<joint name="panda_1_joint1" value="0"/>
- <joint name="panda_1_joint2" value="0"/>
+ <joint name="panda_1_joint2" value="-0.785"/>
<joint name="panda_1_joint3" value="0"/>
- <joint name="panda_1_joint4" value="-1.5708"/>
+ <joint name="panda_1_joint4" value="-2.356"/>
<joint name="panda_1_joint5" value="0"/>
- <joint name="panda_1_joint6" value="0"/>
- <joint name="panda_1_joint7" value="0"/>
+ <joint name="panda_1_joint6" value="1.571"/>
+ <joint name="panda_1_joint7" value="0.780"/>
</group_state>
- <group_state name="home2" group="panda_arm2">
+ <group_state name="ready" group="panda_arm2">
<joint name="panda_2_joint1" value="0"/>
- <joint name="panda_2_joint2" value="0"/>
+ <joint name="panda_2_joint2" value="-0.785"/>
<joint name="panda_2_joint3" value="0"/>
- <joint name="panda_2_joint4" value="-1.5708"/>
+ <joint name="panda_2_joint4" value="-2.356"/>
<joint name="panda_2_joint5" value="0"/>
- <joint name="panda_2_joint6" value="0"/>
- <joint name="panda_2_joint7" value="0"/>
+ <joint name="panda_2_joint6" value="1.571"/>
+ <joint name="panda_2_joint7" value="0.785"/>
</group_state>
+
+ <group_state name="open" group="hand_1">
+ <joint name="panda_1_finger_joint1" value="0.035" />
+ <joint name="panda_1_finger_joint2" value="0.035" />
+ </group_state>
+ <group_state name="close" group="hand_1">
+ <joint name="panda_1_finger_joint1" value="0" />
+ <joint name="panda_1_finger_joint2" value="0" />
+ </group_state>
+
+ <group_state name="open" group="hand_2">
+ <joint name="panda_2_finger_joint1" value="0.035" />
+ <joint name="panda_2_finger_joint2" value="0.035" />
+ </group_state>
+ <group_state name="close" group="hand_2">
+ <joint name="panda_2_finger_joint1" value="0" />
+ <joint name="panda_2_finger_joint2" value="0" />
+ </group_state>
+
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
- <end_effector name="hand1" parent_link="panda_1_link8" group="hand_1"/>
- <end_effector name="hand2" parent_link="panda_2_link8" group="hand_2"/>
+ <end_effector name="hand_1" parent_link="panda_1_link8" group="hand_1"/>
+ <end_effector name="hand_2" parent_link="panda_2_link8" group="hand_2"/>
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="virtual_joint1" type="fixed" parent_frame="world" child_link="base_1"/>
<virtual_joint name="virtual_joint2" type="fixed" parent_frame="world" child_link="base_2"/>
diff --git a/src/ceti_double/launch/moveit_rviz.launch b/src/ceti_double/launch/moveit_rviz.launch
index a4605c03742afe6fe27bcefd811992502a4015b8..1f8241dcc4aac38af2269a8edd74d5743fbb91c7 100644
--- a/src/ceti_double/launch/moveit_rviz.launch
+++ b/src/ceti_double/launch/moveit_rviz.launch
@@ -9,7 +9,9 @@
<arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />
<node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
- args="$(arg command_args)" output="screen">
+ args="$(arg command_args)" output="screen">
+ <rosparam command="load" file="$(find ceti_double)/config/kinematics.yaml"/>
+
</node>
</launch>
diff --git a/src/franka_description/robots/ceti_double.urdf.xacro b/src/franka_description/robots/ceti_double.urdf.xacro
index b3dd77ef1a68d3efb4e4b71988eb475e12ffd11d..df7a8e869aeb2940c8d34c380e9df53644392c33 100644
--- a/src/franka_description/robots/ceti_double.urdf.xacro
+++ b/src/franka_description/robots/ceti_double.urdf.xacro
@@ -10,7 +10,7 @@
<link name="world" />
- <xacro:property name="yaml_file" value="$(find mtc)/results/-668288260.yaml" />
+ <xacro:property name="yaml_file" value="$(find mtc)/results/-671106318.yaml" />
<xacro:property name="props" value="${xacro.load_yaml(yaml_file)}" />
<xacro:property name="x1" value="${props['Robot_arm1']['pos_x']}" />
diff --git a/src/franka_description/robots/panda_arm_hand.urdf (copy).xacro b/src/franka_description/robots/panda_arm_hand.urdf (copy).xacro
new file mode 100644
index 0000000000000000000000000000000000000000..5c424c2c3d257e77585abaa5464325916388fd48
--- /dev/null
+++ b/src/franka_description/robots/panda_arm_hand.urdf (copy).xacro
@@ -0,0 +1,44 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+ <xacro:include filename="panda_arm.xacro"/>
+ <xacro:include filename="hand.xacro"/>
+
+ <!-- for simulation -->
+ <xacro:include filename="panda.gazebo.xacro"/>
+ <xacro:include filename="panda.transmission.xacro"/>
+
+ <link name="base">
+ <visual>
+ <origin xyz="0 0 0.4425" rpy="0 0 0"/>
+ <geometry>
+ <box size="0.8 0.8 0.885" />
+ </geometry>
+ <material name="White">
+ <color rgba="1.0 1.0 1.0 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0.4425" rpy="0 0 0"/>
+ <geometry>
+ <box size="0.8 0.8 0.885" />
+ </geometry>
+ </collision>
+ </link>
+
+ <xacro:arg name="GOAL" default='Base_task'/>
+
+ <xacro:property name="yaml_file" value="$(find reachability)/Positionsanalyse/$(arg GOAL).yaml" />
+ <xacro:property name="props" value="${load_yaml(yaml_file)}" />
+
+ <xacro:property name="pos_1" value="${props['Basispositionen']['panda_arm1']}" />
+
+ <xacro:panda_arm arm_id="panda" connected_to="base" xyz="-0.22 0 0.885"/>
+ <xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8"/>
+
+
+ <!-- for simulation -->
+ <xacro:arg name="robot_name" default="panda"/>
+ <xacro:panda_gazebo robot_name="$(arg robot_name)" />
+ <xacro:panda_transmission robot_name="$(arg robot_name)" load_hand="true" />
+ <!-- end for simulation -->
+</robot>
diff --git a/src/franka_description/robots/panda_arm_hand.urdf.xacro b/src/franka_description/robots/panda_arm_hand.urdf.xacro
index 5c424c2c3d257e77585abaa5464325916388fd48..65ac6b9fa17f55ffe5c22b90f4547293c575757a 100644
--- a/src/franka_description/robots/panda_arm_hand.urdf.xacro
+++ b/src/franka_description/robots/panda_arm_hand.urdf.xacro
@@ -1,40 +1,15 @@
<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
- <xacro:include filename="panda_arm.xacro"/>
- <xacro:include filename="hand.xacro"/>
-
+ <xacro:include filename="$(find franka_description)/robots/panda_arm.xacro"/>
+ <xacro:include filename="$(find franka_description)/robots/hand.xacro"/>
+
<!-- for simulation -->
- <xacro:include filename="panda.gazebo.xacro"/>
+ <xacro:include filename="$(find franka_description)/robots/panda.gazebo.xacro"/>
<xacro:include filename="panda.transmission.xacro"/>
-
- <link name="base">
- <visual>
- <origin xyz="0 0 0.4425" rpy="0 0 0"/>
- <geometry>
- <box size="0.8 0.8 0.885" />
- </geometry>
- <material name="White">
- <color rgba="1.0 1.0 1.0 1.0"/>
- </material>
- </visual>
- <collision>
- <origin xyz="0 0 0.4425" rpy="0 0 0"/>
- <geometry>
- <box size="0.8 0.8 0.885" />
- </geometry>
- </collision>
- </link>
-
- <xacro:arg name="GOAL" default='Base_task'/>
-
- <xacro:property name="yaml_file" value="$(find reachability)/Positionsanalyse/$(arg GOAL).yaml" />
- <xacro:property name="props" value="${load_yaml(yaml_file)}" />
+ <!-- end for simulation -->
- <xacro:property name="pos_1" value="${props['Basispositionen']['panda_arm1']}" />
-
- <xacro:panda_arm arm_id="panda" connected_to="base" xyz="-0.22 0 0.885"/>
+ <xacro:panda_arm />
<xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8"/>
-
<!-- for simulation -->
<xacro:arg name="robot_name" default="panda"/>
diff --git a/src/mtc/CMakeLists.txt b/src/mtc/CMakeLists.txt
index 1b18ded04128a426f68dc9b5cf24d16a4cd922d1..6442dfa28ccd37b8cde894bf42e1954bd449f163 100644
--- a/src/mtc/CMakeLists.txt
+++ b/src/mtc/CMakeLists.txt
@@ -18,6 +18,9 @@ find_package(OpenMP)
# Load catkin and all dependencies required for this package
find_package(catkin REQUIRED COMPONENTS
+ moveit_task_constructor_core
+ moveit_task_constructor_visualization
+ moveit_task_constructor_msgs
eigen_conversions
geometry_msgs
message_generation
@@ -34,6 +37,8 @@ find_package(catkin REQUIRED COMPONENTS
tf_conversions
trajectory_msgs
pcl_ros
+ yaml_to_mtc
+
)
# Catkin
@@ -79,6 +84,7 @@ src/impl/moveit_mediator.cpp
src/impl/robot.cpp
src/impl/wing_moveit_decorator.cpp
src/impl/wing_rviz_decorator.cpp
+src/impl/collision_helper.cpp
)
add_executable(cell_routine src/cell_routine.cpp
@@ -90,6 +96,7 @@ src/impl/robot.cpp
src/impl/mediator.cpp
src/impl/wing_moveit_decorator.cpp
src/impl/wing_rviz_decorator.cpp
+src/impl/collision_helper.cpp
)
diff --git a/src/mtc/include/impl/abstract_map_loader.h b/src/mtc/include/impl/abstract_map_loader.h
index e98aaf6406d17ae09612977900a44b2a5d7952ef..0390955c80d7da9227da185d56ba4c17a271033d 100644
--- a/src/mtc/include/impl/abstract_map_loader.h
+++ b/src/mtc/include/impl/abstract_map_loader.h
@@ -9,6 +9,11 @@
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
+
+#include <ros/package.h>
+#include <yaml-cpp/yaml.h>
+#include <fstream>
+
#include "impl/abstract_strategy.h"
class Abstract_strategy;
@@ -40,10 +45,9 @@ class Abstract_map_loader{
inline void set_target_rot (std::vector<std::vector<std::vector<tf2::Quaternion>>>& rots) {target_rot_ = rots;}
inline void set_target_cloud(std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>& cloud) {target_cloud_ = cloud;}
inline std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>& target_cloud () { return target_cloud_;}
-
-
static std::vector<pcl::PointXYZ> create_pcl_box();
+ virtual void write_task()=0;
virtual std::vector<std::vector<pcl::PointXYZ>> base_calculation()=0;
};
#endif
diff --git a/src/mtc/include/impl/abstract_mediator.h b/src/mtc/include/impl/abstract_mediator.h
index d9a161b0cafb770de5395abd005ebfbdac0f138c..9d63a7a27e202348a1774e2f67047202dca80fbc 100644
--- a/src/mtc/include/impl/abstract_mediator.h
+++ b/src/mtc/include/impl/abstract_mediator.h
@@ -41,9 +41,9 @@ class Abstract_mediator {
public:
Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){
- wings_.push_back(wing_BP(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right));
- wings_.push_back(wing_BP(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.6525f,0,0.4425f-0.005f)), mid));
- wings_.push_back(wing_BP(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.6525f,0.4425f -0.005f)), left));
+ wings_.push_back(wing_BP(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right_size));
+ wings_.push_back(wing_BP(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.6525f,0,0.4425f-0.005f)), mid_size));
+ wings_.push_back(wing_BP(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.6525f,0.4425f -0.005f)), left_size));
}
inline void connect_robots(Abstract_robot* robot) {robots_.push_back(robot); ROS_INFO("%s connected...", robot->name().c_str());}
diff --git a/src/mtc/include/impl/abstract_robot.h b/src/mtc/include/impl/abstract_robot.h
index d8a2823e7a2a0e12e2b2493776ed5610a1d8ca04..499ae7b9f16a09d38f1a0cb33ca5e4e5e7564cee 100644
--- a/src/mtc/include/impl/abstract_robot.h
+++ b/src/mtc/include/impl/abstract_robot.h
@@ -7,9 +7,9 @@
#include "impl/abstract_strategy.h"
#include "impl/abstract_robot_element.h"
-#define right tf2::Vector3(0.7f, 0.5f, 0.01f)
-#define left tf2::Vector3(0.7f, 0.5f, 0.01f)
-#define mid tf2::Vector3(0.5f, 0.7f, 0.01f)
+#define right_size tf2::Vector3(0.7f, 0.5f, 0.01f)
+#define left_size tf2::Vector3(0.7f, 0.5f, 0.01f)
+#define mid_size tf2::Vector3(0.5f, 0.7f, 0.01f)
enum wing_config{
RML = 7,
diff --git a/src/mtc/include/impl/collision_helper.h b/src/mtc/include/impl/collision_helper.h
new file mode 100644
index 0000000000000000000000000000000000000000..f14e1997ff2763c61ab588679e5000388070bcf7
--- /dev/null
+++ b/src/mtc/include/impl/collision_helper.h
@@ -0,0 +1,18 @@
+#ifndef COLLISION_HELPER_
+#define COLLISION_HELPER_
+
+#include <ros/ros.h>
+#include <moveit/collision_detection/collision_env.h>
+#include <moveit/collision_detection_fcl/collision_env_fcl.h>
+
+
+
+class Collision_helper : public collision_detection::CollisionEnvFCL{
+ public:
+ Collision_helper()= default;
+ ~Collision_helper()= default;
+
+ bool collision_detector(const std::string& object1_name, const std::string& object2_name);
+};
+
+#endif
\ No newline at end of file
diff --git a/src/mtc/include/impl/map_loader.h b/src/mtc/include/impl/map_loader.h
index 2e9c208049322f55792e027130b1149ceb86c83f..d50aae757558443f876cdefe0e16d7fdf17c8257 100644
--- a/src/mtc/include/impl/map_loader.h
+++ b/src/mtc/include/impl/map_loader.h
@@ -6,12 +6,12 @@
#include <regex>
+
class Map_loader : public Abstract_map_loader {
public:
Map_loader(XmlRpc::XmlRpcValue& map_data, XmlRpc::XmlRpcValue& target_data);
std::vector<std::vector<pcl::PointXYZ>> base_calculation() override;
+ void write_task() override;
};
-
-
#endif
\ No newline at end of file
diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch
index 4999cebde4b7275a4608a030f4ee87139554beb0..60298dd966f01d7ca33730685b097861c0fc57c7 100644
--- a/src/mtc/launch/cell_routine.launch
+++ b/src/mtc/launch/cell_routine.launch
@@ -1,9 +1,12 @@
<launch>
<!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
- <rosparam command="load" file="$(find mtc)/results/-668288260.yaml"/>
+ <rosparam command="load" file="$(find mtc)/results/-671106318.yaml"/>
+ <rosparam command="load" file="$(find mtc)/mtc_task_file/dummy.yaml" />
- <include file="$(find ceti_double)/launch/demo.launch">
- </include>
+ <!--<include file="$(find ceti_double)/launch/moveit_rviz.launch"> -->
+ <!--</include> -->
- <node pkg="mtc" type="cell_routine" name="cell_routine" output="screen" required="true"/>
-</launch>
\ No newline at end of file
+ <node pkg="mtc" type="cell_routine" name="cell_routine" output="screen" required="true" />
+</launch>
+
+
diff --git a/src/mtc/launch/rviz/cell_routine.rviz b/src/mtc/launch/rviz/cell_routine.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..02da6ef90bb6f74f21e5a7e92e620aed1a032e96
--- /dev/null
+++ b/src/mtc/launch/rviz/cell_routine.rviz
@@ -0,0 +1,180 @@
+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
diff --git a/src/mtc/mtc_task_file/dummy.yaml b/src/mtc/mtc_task_file/dummy.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ddf5b49a4ff369c05f6e43c180d179646228b4b3
--- /dev/null
+++ b/src/mtc/mtc_task_file/dummy.yaml
@@ -0,0 +1,208 @@
+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/body
+ 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.4f, y: 0.5f, z: 0.1}
+ 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
diff --git a/src/mtc/package.xml b/src/mtc/package.xml
index bea7c8f85b4e24a9ecbac5110921c1f8a24fed18..cd7cd58d6af1f909d2e16230737f9a0386f8c3b8 100644
--- a/src/mtc/package.xml
+++ b/src/mtc/package.xml
@@ -36,6 +36,7 @@
<build_depend>geometry_msgs</build_depend>
<build_depend>YAML_CPP</build_depend>
<build_depend>pcl_ros</build_depend>
+ <build_depend>yaml_to_mtc</build_depend>
diff --git a/src/mtc/results/-1046057965.yaml b/src/mtc/results/-1046057965.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4097c16da3c6d488a22782c4b61329ec980dbd8c
--- /dev/null
+++ b/src/mtc/results/-1046057965.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.04361940470594575
+ q_w: 0.9990482208247503
+ rpy: 0.000000 -0.000000 0.087266
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1137403263029958
+ pos_y: 1.300031983349814
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.7372773633019241
+ q_w: 0.6755901787049325
+ rpy: 0.000000 -0.000000 1.658063
+ wings: 7
\ No newline at end of file
diff --git a/src/mtc/results/-1055738997.yaml b/src/mtc/results/-1055738997.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..9d7244eee43769cf79cae4afeac3769de75ae3bd
--- /dev/null
+++ b/src/mtc/results/-1055738997.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.04361940470594575
+ q_w: 0.9990482208247503
+ rpy: 0.000000 -0.000000 0.087266
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1137403263029958
+ pos_y: 1.300031983349814
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.7372773633019241
+ q_w: 0.6755901787049325
+ rpy: 0.000000 -0.000000 1.658063
+ wings: 6
\ No newline at end of file
diff --git a/src/mtc/results/-1080103651.yaml b/src/mtc/results/-1080103651.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3d1322deaeb457a07a16e82cfee72f0cebf4b6fc
--- /dev/null
+++ b/src/mtc/results/-1080103651.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.04361940470594575
+ q_w: 0.9990482208247503
+ rpy: 0.000000 -0.000000 0.087266
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1137403263029958
+ pos_y: 1.300031983349814
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.04361949204551854
+ q_w: 0.9990482170114167
+ rpy: 0.000000 -0.000000 0.087267
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/2065375902.yaml b/src/mtc/results/-1189754863.yaml
similarity index 100%
rename from src/mtc/results/2065375902.yaml
rename to src/mtc/results/-1189754863.yaml
diff --git a/src/mtc/results/2051106760.yaml b/src/mtc/results/-1207470325.yaml
similarity index 100%
rename from src/mtc/results/2051106760.yaml
rename to src/mtc/results/-1207470325.yaml
diff --git a/src/mtc/results/-1216992529.yaml b/src/mtc/results/-1216992529.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f740380697cdfbf24d9184cd9060d6148db607f4
--- /dev/null
+++ b/src/mtc/results/-1216992529.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/-1237789212.yaml b/src/mtc/results/-1237789212.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d16a10f310038abb33a6cc6ebf13b18178a8641f
--- /dev/null
+++ b/src/mtc/results/-1237789212.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/1899539074.yaml b/src/mtc/results/-1421451943.yaml
similarity index 100%
rename from src/mtc/results/1899539074.yaml
rename to src/mtc/results/-1421451943.yaml
diff --git a/src/mtc/results/1885436442.yaml b/src/mtc/results/-1439353962.yaml
similarity index 100%
rename from src/mtc/results/1885436442.yaml
rename to src/mtc/results/-1439353962.yaml
diff --git a/src/mtc/results/-1448414209.yaml b/src/mtc/results/-1448414209.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6f693cbc82b50f5d221b868f09dc56bf3bb8dfe2
--- /dev/null
+++ b/src/mtc/results/-1448414209.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -0.2000020444393158
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.104997903104521
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/1871941323.yaml b/src/mtc/results/-1457036108.yaml
similarity index 100%
rename from src/mtc/results/1871941323.yaml
rename to src/mtc/results/-1457036108.yaml
diff --git a/src/mtc/results/-1466176747.yaml b/src/mtc/results/-1466176747.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d2cad5f0ac097d0531ebbf5f1841aece379bbcaf
--- /dev/null
+++ b/src/mtc/results/-1466176747.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -0.2000020444393158
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.104997903104521
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/1859157926.yaml b/src/mtc/results/-1474038940.yaml
similarity index 100%
rename from src/mtc/results/1859157926.yaml
rename to src/mtc/results/-1474038940.yaml
diff --git a/src/mtc/results/-1887327637.yaml b/src/mtc/results/-1887327637.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d6326bd8732f9e675ba5dc60400a5327b4e2859f
--- /dev/null
+++ b/src/mtc/results/-1887327637.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 7
\ No newline at end of file
diff --git a/src/mtc/results/-1896797876.yaml b/src/mtc/results/-1896797876.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..59c87bdeac32b94c5545ad1bec5e03c7e666c55f
--- /dev/null
+++ b/src/mtc/results/-1896797876.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 6
\ No newline at end of file
diff --git a/src/mtc/results/-1921624487.yaml b/src/mtc/results/-1921624487.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b3c009b9ad19cc5d4dfc49161df4506cf1110abb
--- /dev/null
+++ b/src/mtc/results/-1921624487.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/-1922181432.yaml b/src/mtc/results/-1922181432.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d0376b0c3fab24db688b9bfdbd392e512f9994af
--- /dev/null
+++ b/src/mtc/results/-1922181432.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/-1931011330.yaml b/src/mtc/results/-1931011330.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5075ef070926d49c582036f8ce3c5c48bd0f6424
--- /dev/null
+++ b/src/mtc/results/-1931011330.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 2
\ No newline at end of file
diff --git a/src/mtc/results/-1938789709.yaml b/src/mtc/results/-1938789709.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..65c7703a8ba883d64850d8e69363fd5cdc2cdf50
--- /dev/null
+++ b/src/mtc/results/-1938789709.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/1456397962.yaml b/src/mtc/results/-1961866838.yaml
similarity index 100%
rename from src/mtc/results/1456397962.yaml
rename to src/mtc/results/-1961866838.yaml
diff --git a/src/mtc/results/1448889126.yaml b/src/mtc/results/-1971205279.yaml
similarity index 100%
rename from src/mtc/results/1448889126.yaml
rename to src/mtc/results/-1971205279.yaml
diff --git a/src/mtc/results/1441695710.yaml b/src/mtc/results/-1980533871.yaml
similarity index 100%
rename from src/mtc/results/1441695710.yaml
rename to src/mtc/results/-1980533871.yaml
diff --git a/src/mtc/results/1434971863.yaml b/src/mtc/results/-1988573751.yaml
similarity index 100%
rename from src/mtc/results/1434971863.yaml
rename to src/mtc/results/-1988573751.yaml
diff --git a/src/mtc/results/-1989815598.yaml b/src/mtc/results/-1989815598.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1d44d1272157ceced31f84cb995379eec5cc50ad
--- /dev/null
+++ b/src/mtc/results/-1989815598.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361815618200975
+ q_w: 0.9990482753357227
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 7
+Robot_arm2:
+ pos_x: 0.01373299255459678
+ pos_y: 1.400032269116376
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361806884243188
+ q_w: 0.9990482791489393
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/-2007051923.yaml b/src/mtc/results/-2007051923.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..fef1d6df3adce61bd395e533414111fee24130b0
--- /dev/null
+++ b/src/mtc/results/-2007051923.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361815618200975
+ q_w: 0.9990482753357227
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 7
+Robot_arm2:
+ pos_x: 0.01373299255459678
+ pos_y: 1.400032269116376
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361806884243188
+ q_w: 0.9990482791489393
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/-1770475312.yaml b/src/mtc/results/-623868902.yaml
similarity index 100%
rename from src/mtc/results/-1770475312.yaml
rename to src/mtc/results/-623868902.yaml
diff --git a/src/mtc/results/-1777977373.yaml b/src/mtc/results/-633538828.yaml
similarity index 100%
rename from src/mtc/results/-1777977373.yaml
rename to src/mtc/results/-633538828.yaml
diff --git a/src/mtc/results/-1786268055.yaml b/src/mtc/results/-642434800.yaml
similarity index 100%
rename from src/mtc/results/-1786268055.yaml
rename to src/mtc/results/-642434800.yaml
diff --git a/src/mtc/results/-1796122988.yaml b/src/mtc/results/-650637628.yaml
similarity index 100%
rename from src/mtc/results/-1796122988.yaml
rename to src/mtc/results/-650637628.yaml
diff --git a/src/mtc/results/-651802785.yaml b/src/mtc/results/-651802785.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..9913e3de9ab1f4348a56e1241c2ef823fc7cb30e
--- /dev/null
+++ b/src/mtc/results/-651802785.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/-671106318.yaml b/src/mtc/results/-671106318.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d2b122c775ed1da774d7b9c4b4aa3e061df79a28
--- /dev/null
+++ b/src/mtc/results/-671106318.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/-831518075.yaml b/src/mtc/results/-831518075.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c0129958bfb39193745e2277fe14b5c3d2200474
--- /dev/null
+++ b/src/mtc/results/-831518075.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.04362065322981368
+ q_w: 0.9990481663122177
+ rpy: 0.000000 -0.000000 0.087269
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1137435756424012
+ pos_y: 1.300031699065247
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.7372782075954164
+ q_w: 0.6755892573190391
+ rpy: 0.000000 -0.000000 1.658065
+ wings: 7
\ No newline at end of file
diff --git a/src/mtc/results/-841643951.yaml b/src/mtc/results/-841643951.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..04a81cb387de97e9656031c69caa19f2ebdd0002
--- /dev/null
+++ b/src/mtc/results/-841643951.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.04362065322981368
+ q_w: 0.9990481663122177
+ rpy: 0.000000 -0.000000 0.087269
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1137435756424012
+ pos_y: 1.300031699065247
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.7372782075954164
+ q_w: 0.6755892573190391
+ rpy: 0.000000 -0.000000 1.658065
+ wings: 6
\ No newline at end of file
diff --git a/src/mtc/results/-868320620.yaml b/src/mtc/results/-868320620.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e85d8e4a26e4c2d0c1be26b8b0f6cc8ff715bc17
--- /dev/null
+++ b/src/mtc/results/-868320620.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.04362065322981368
+ q_w: 0.9990481663122177
+ rpy: 0.000000 -0.000000 0.087269
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1137435756424012
+ pos_y: 1.300031699065247
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.0436207405693817
+ q_w: 0.9990481624987751
+ rpy: 0.000000 -0.000000 0.087269
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/-2009327429.yaml b/src/mtc/results/-909859228.yaml
similarity index 100%
rename from src/mtc/results/-2009327429.yaml
rename to src/mtc/results/-909859228.yaml
diff --git a/src/mtc/results/-2016725910.yaml b/src/mtc/results/-919140465.yaml
similarity index 100%
rename from src/mtc/results/-2016725910.yaml
rename to src/mtc/results/-919140465.yaml
diff --git a/src/mtc/results/-2023885311.yaml b/src/mtc/results/-928584791.yaml
similarity index 100%
rename from src/mtc/results/-2023885311.yaml
rename to src/mtc/results/-928584791.yaml
diff --git a/src/mtc/results/-2030540640.yaml b/src/mtc/results/-936863471.yaml
similarity index 100%
rename from src/mtc/results/-2030540640.yaml
rename to src/mtc/results/-936863471.yaml
diff --git a/src/mtc/results/-938048744.yaml b/src/mtc/results/-938048744.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..38f2954ab4be0c2ec4c677740cd1acc353591e0b
--- /dev/null
+++ b/src/mtc/results/-938048744.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/-955345555.yaml b/src/mtc/results/-955345555.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5bcdc9d08f1f70c2ba73a5139a495150f081758e
--- /dev/null
+++ b/src/mtc/results/-955345555.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/1001040782.yaml b/src/mtc/results/1001040782.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..cf72c103523290eb8d5585535bb506d15b95a8f5
--- /dev/null
+++ b/src/mtc/results/1001040782.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -0.2000020444393158
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.104997903104521
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/1392124514.yaml b/src/mtc/results/1010572206.yaml
similarity index 100%
rename from src/mtc/results/1392124514.yaml
rename to src/mtc/results/1010572206.yaml
diff --git a/src/mtc/results/1407514948.yaml b/src/mtc/results/1029257752.yaml
similarity index 100%
rename from src/mtc/results/1407514948.yaml
rename to src/mtc/results/1029257752.yaml
diff --git a/src/mtc/results/1207546769.yaml b/src/mtc/results/1207546769.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..957500bda87ec9e8a5fd43664caf280cdb4faff4
--- /dev/null
+++ b/src/mtc/results/1207546769.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/1226698392.yaml b/src/mtc/results/1226698392.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6da15ba8d1d115b9528ec8afb81ce17141734168
--- /dev/null
+++ b/src/mtc/results/1226698392.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/1569464609.yaml b/src/mtc/results/1236800871.yaml
similarity index 100%
rename from src/mtc/results/1569464609.yaml
rename to src/mtc/results/1236800871.yaml
diff --git a/src/mtc/results/1584757889.yaml b/src/mtc/results/1257095161.yaml
similarity index 100%
rename from src/mtc/results/1584757889.yaml
rename to src/mtc/results/1257095161.yaml
diff --git a/src/mtc/results/1374844943.yaml b/src/mtc/results/1374844943.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..dc5ea136f6135909fffff98c151c8e09b54eeee3
--- /dev/null
+++ b/src/mtc/results/1374844943.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.04361940470594575
+ q_w: 0.9990482208247503
+ rpy: 0.000000 -0.000000 0.087266
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1137403263029958
+ pos_y: 1.300031983349814
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.04361949204551854
+ q_w: 0.9990482170114167
+ rpy: 0.000000 -0.000000 0.087267
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/1402747331.yaml b/src/mtc/results/1402747331.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..43ba4473d82d50a7bf279cbbf4261fc3f42bb5bd
--- /dev/null
+++ b/src/mtc/results/1402747331.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.04361940470594575
+ q_w: 0.9990482208247503
+ rpy: 0.000000 -0.000000 0.087266
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1137403263029958
+ pos_y: 1.300031983349814
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.7372773633019241
+ q_w: 0.6755901787049325
+ rpy: 0.000000 -0.000000 1.658063
+ wings: 6
\ No newline at end of file
diff --git a/src/mtc/results/1413304642.yaml b/src/mtc/results/1413304642.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..02d0e870a62bc8b56199acddaf0f1b709064db95
--- /dev/null
+++ b/src/mtc/results/1413304642.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.04361940470594575
+ q_w: 0.9990482208247503
+ rpy: 0.000000 -0.000000 0.087266
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1137403263029958
+ pos_y: 1.300031983349814
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.7372773633019241
+ q_w: 0.6755901787049325
+ rpy: 0.000000 -0.000000 1.658063
+ wings: 7
\ No newline at end of file
diff --git a/src/mtc/results/1476708103.yaml b/src/mtc/results/1476708103.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5ac792dd22d9afd0040eb8ef62f3cffdf2889e4c
--- /dev/null
+++ b/src/mtc/results/1476708103.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/1484564639.yaml b/src/mtc/results/1484564639.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..937736980de2017d734d99b3404f3399dd9e90ec
--- /dev/null
+++ b/src/mtc/results/1484564639.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 2
\ No newline at end of file
diff --git a/src/mtc/results/1493265533.yaml b/src/mtc/results/1493265533.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0a69f3898c9a110cd9ff134cfc8eb821e125c30a
--- /dev/null
+++ b/src/mtc/results/1493265533.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/1493843502.yaml b/src/mtc/results/1493843502.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..041a044767447be17b937063afaad4824e0fae65
--- /dev/null
+++ b/src/mtc/results/1493843502.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/1508564270.yaml b/src/mtc/results/1508564270.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1041d8c7782f966961137b9260b0bb2cc19991d6
--- /dev/null
+++ b/src/mtc/results/1508564270.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/1522298431.yaml b/src/mtc/results/1522298431.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..94fc278462df60e05671abef6c5eeebd98518f65
--- /dev/null
+++ b/src/mtc/results/1522298431.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 6
\ No newline at end of file
diff --git a/src/mtc/results/1527465428.yaml b/src/mtc/results/1527465428.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1f2b42379d147f4c3dc39e1258d04aa92dc600ec
--- /dev/null
+++ b/src/mtc/results/1527465428.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/1794683721.yaml b/src/mtc/results/1528640783.yaml
similarity index 100%
rename from src/mtc/results/1794683721.yaml
rename to src/mtc/results/1528640783.yaml
diff --git a/src/mtc/results/1531997134.yaml b/src/mtc/results/1531997134.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3f23756c80a1b23671ee82bfc043410b94bf1254
--- /dev/null
+++ b/src/mtc/results/1531997134.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 7
\ No newline at end of file
diff --git a/src/mtc/results/1801771460.yaml b/src/mtc/results/1537391338.yaml
similarity index 100%
rename from src/mtc/results/1801771460.yaml
rename to src/mtc/results/1537391338.yaml
diff --git a/src/mtc/results/1809636855.yaml b/src/mtc/results/1547395545.yaml
similarity index 100%
rename from src/mtc/results/1809636855.yaml
rename to src/mtc/results/1547395545.yaml
diff --git a/src/mtc/results/1817405794.yaml b/src/mtc/results/1557145865.yaml
similarity index 100%
rename from src/mtc/results/1817405794.yaml
rename to src/mtc/results/1557145865.yaml
diff --git a/src/mtc/results/1600170366.yaml b/src/mtc/results/1600170366.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b4306e0f008c4fa31bf84a18fb62c91c528f215d
--- /dev/null
+++ b/src/mtc/results/1600170366.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.04362065322981368
+ q_w: 0.9990481663122177
+ rpy: 0.000000 -0.000000 0.087269
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1137435756424012
+ pos_y: 1.300031699065247
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.0436207405693817
+ q_w: 0.9990481624987751
+ rpy: 0.000000 -0.000000 0.087269
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/1627308507.yaml b/src/mtc/results/1627308507.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4997617b8a4c758b53fd2a12160e0389534a7816
--- /dev/null
+++ b/src/mtc/results/1627308507.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.04362065322981368
+ q_w: 0.9990481663122177
+ rpy: 0.000000 -0.000000 0.087269
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1137435756424012
+ pos_y: 1.300031699065247
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.7372782075954164
+ q_w: 0.6755892573190391
+ rpy: 0.000000 -0.000000 1.658065
+ wings: 6
\ No newline at end of file
diff --git a/src/mtc/results/1638136608.yaml b/src/mtc/results/1638136608.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..bc48e32480f0056c90493dac3836e48201f658cd
--- /dev/null
+++ b/src/mtc/results/1638136608.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.04362065322981368
+ q_w: 0.9990481663122177
+ rpy: 0.000000 -0.000000 0.087269
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1137435756424012
+ pos_y: 1.300031699065247
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.7372782075954164
+ q_w: 0.6755892573190391
+ rpy: 0.000000 -0.000000 1.658065
+ wings: 7
\ No newline at end of file
diff --git a/src/mtc/results/1811782592.yaml b/src/mtc/results/1811782592.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..821a164afcb6ed53c7a2bf6c1cbccaca9602f6aa
--- /dev/null
+++ b/src/mtc/results/1811782592.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/1834542208.yaml b/src/mtc/results/1834542208.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..93023524a9b3b19a9723cfd92e7a5a43cda24a54
--- /dev/null
+++ b/src/mtc/results/1834542208.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/2029203334.yaml b/src/mtc/results/1836074751.yaml
similarity index 100%
rename from src/mtc/results/2029203334.yaml
rename to src/mtc/results/1836074751.yaml
diff --git a/src/mtc/results/2036675920.yaml b/src/mtc/results/1845125711.yaml
similarity index 100%
rename from src/mtc/results/2036675920.yaml
rename to src/mtc/results/1845125711.yaml
diff --git a/src/mtc/results/2045443294.yaml b/src/mtc/results/1854984640.yaml
similarity index 100%
rename from src/mtc/results/2045443294.yaml
rename to src/mtc/results/1854984640.yaml
diff --git a/src/mtc/results/2053540854.yaml b/src/mtc/results/1864787762.yaml
similarity index 100%
rename from src/mtc/results/2053540854.yaml
rename to src/mtc/results/1864787762.yaml
diff --git a/src/mtc/results/1115810607.yaml b/src/mtc/results/1873505519.yaml
similarity index 100%
rename from src/mtc/results/1115810607.yaml
rename to src/mtc/results/1873505519.yaml
diff --git a/src/mtc/results/1130176764.yaml b/src/mtc/results/1892447883.yaml
similarity index 100%
rename from src/mtc/results/1130176764.yaml
rename to src/mtc/results/1892447883.yaml
diff --git a/src/mtc/results/1916980725.yaml b/src/mtc/results/1916980725.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..012b352cee9c7b5dfd01b6ee882f0a5cf9d375b3
--- /dev/null
+++ b/src/mtc/results/1916980725.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/1925434018.yaml b/src/mtc/results/1925434018.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..cf3cb44e3d3b471cd5bfbfb1337f75729c9e8296
--- /dev/null
+++ b/src/mtc/results/1925434018.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 2
\ No newline at end of file
diff --git a/src/mtc/results/1934936595.yaml b/src/mtc/results/1934936595.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3bfbb8bf7090fe9b0124170d532ea90d63668c16
--- /dev/null
+++ b/src/mtc/results/1934936595.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/1935481946.yaml b/src/mtc/results/1935481946.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a948b397a77ba42f800f74145bfdcd448ad8cff1
--- /dev/null
+++ b/src/mtc/results/1935481946.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/1960603164.yaml b/src/mtc/results/1960603164.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d81bc9e696c77f6b423b84cf0cf7709752a93eaa
--- /dev/null
+++ b/src/mtc/results/1960603164.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 6
\ No newline at end of file
diff --git a/src/mtc/results/1970178729.yaml b/src/mtc/results/1970178729.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..82a772d0fb2535bca5eb033cc943e43b97234daf
--- /dev/null
+++ b/src/mtc/results/1970178729.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 7
\ No newline at end of file
diff --git a/src/mtc/results/408216677.yaml b/src/mtc/results/408216677.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7b44ef5a16a3c17974125d6208a36ea8a6063d72
--- /dev/null
+++ b/src/mtc/results/408216677.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/431123736.yaml b/src/mtc/results/431123736.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d4010d0ac2c3ec7994ae456b50ec7c904045a5d7
--- /dev/null
+++ b/src/mtc/results/431123736.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 2
\ No newline at end of file
diff --git a/src/mtc/results/440587132.yaml b/src/mtc/results/440587132.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5858af0481dd0a5a768ae97a1041e506ab8f2c82
--- /dev/null
+++ b/src/mtc/results/440587132.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/441120610.yaml b/src/mtc/results/441120610.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..574c99653692427812479a98ceda80e7e79521fb
--- /dev/null
+++ b/src/mtc/results/441120610.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/467190469.yaml b/src/mtc/results/467190469.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f29d20bf9df92d466ce02679607a959485998e51
--- /dev/null
+++ b/src/mtc/results/467190469.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 6
\ No newline at end of file
diff --git a/src/mtc/results/477107719.yaml b/src/mtc/results/477107719.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..cd22164f4ffb19bdfe755dd9a98d20122fcca224
--- /dev/null
+++ b/src/mtc/results/477107719.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -0.1000020429491997
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.204997904594638
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 7
\ No newline at end of file
diff --git a/src/mtc/results/510345721.yaml b/src/mtc/results/510345721.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0ee170e584e1bd81087810bcc6bd8539ff1bb38c
--- /dev/null
+++ b/src/mtc/results/510345721.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/522082508.yaml b/src/mtc/results/522082508.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f587627e896bd753ea4fd074907ff7f932c7d1e4
--- /dev/null
+++ b/src/mtc/results/522082508.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 2
\ No newline at end of file
diff --git a/src/mtc/results/532042713.yaml b/src/mtc/results/532042713.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5993d204dc5446702ffa982acafd088946b2a987
--- /dev/null
+++ b/src/mtc/results/532042713.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/532792641.yaml b/src/mtc/results/532792641.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..314c5bbe26d761d206ffec73acd1e245e9fa8360
--- /dev/null
+++ b/src/mtc/results/532792641.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/560606954.yaml b/src/mtc/results/560606954.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..882089405841112241af5f642df54e86b1b8498f
--- /dev/null
+++ b/src/mtc/results/560606954.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 6
\ No newline at end of file
diff --git a/src/mtc/results/570845562.yaml b/src/mtc/results/570845562.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3c4bb8aba3b845b18ceb59a29d290e9ea6933af6
--- /dev/null
+++ b/src/mtc/results/570845562.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 7
\ No newline at end of file
diff --git a/src/mtc/results/591965574.yaml b/src/mtc/results/591965574.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..bb99c8078a5762efc27d9b9bce3ba29dcbdb483f
--- /dev/null
+++ b/src/mtc/results/591965574.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.2000020444393158
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361815618200975
+ q_w: 0.9990482753357227
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 7
+Robot_arm2:
+ pos_x: -0.08626700893551933
+ pos_y: 1.30003226762626
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361806884243188
+ q_w: 0.9990482791489393
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/595297178.yaml b/src/mtc/results/595297178.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6101b88ac8faa6c29ebb2e7b28bd050bc4a6689b
--- /dev/null
+++ b/src/mtc/results/595297178.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/600330373.yaml b/src/mtc/results/600330373.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ae4297da16dc518df03513bc6875183e9b283efb
--- /dev/null
+++ b/src/mtc/results/600330373.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.2000020444393158
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361815618200975
+ q_w: 0.9990482753357227
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 7
+Robot_arm2:
+ pos_x: -0.08626700893551933
+ pos_y: 1.30003226762626
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.6755911323171522
+ q_w: 0.7372764894762539
+ rpy: 0.000000 -0.000000 1.483532
+ wings: 2
\ No newline at end of file
diff --git a/src/mtc/results/603635298.yaml b/src/mtc/results/603635298.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..186deb1f6a21eeffae223dfa143370ce8fa8fae9
--- /dev/null
+++ b/src/mtc/results/603635298.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 2
\ No newline at end of file
diff --git a/src/mtc/results/609980115.yaml b/src/mtc/results/609980115.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b8e1d7e8a56da2c837d0eecbdb37f5376258fbfe
--- /dev/null
+++ b/src/mtc/results/609980115.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.2000020444393158
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361815618200975
+ q_w: 0.9990482753357227
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 7
+Robot_arm2:
+ pos_x: -0.08626700893551933
+ pos_y: 1.30003226762626
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.6755911323171522
+ q_w: 0.7372764894762539
+ rpy: 0.000000 -0.000000 1.483532
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/610757492.yaml b/src/mtc/results/610757492.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..80a8fd08f5fa4e885463727cd758d40796a8a377
--- /dev/null
+++ b/src/mtc/results/610757492.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.2000020444393158
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361815618200975
+ q_w: 0.9990482753357227
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 7
+Robot_arm2:
+ pos_x: -0.08626700893551933
+ pos_y: 1.30003226762626
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361806884243188
+ q_w: 0.9990482791489393
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/613066006.yaml b/src/mtc/results/613066006.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..af6db33287dc3044399dd5e9adc928c02570be37
--- /dev/null
+++ b/src/mtc/results/613066006.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/613727719.yaml b/src/mtc/results/613727719.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c0e078d375a5ff94c836f7a8956b72da253af208
--- /dev/null
+++ b/src/mtc/results/613727719.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/636220253.yaml b/src/mtc/results/636220253.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4415d0fac17fc07aca3d99f8b57e34a5c295d66f
--- /dev/null
+++ b/src/mtc/results/636220253.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.2000020444393158
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361815618200975
+ q_w: 0.9990482753357227
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 7
+Robot_arm2:
+ pos_x: -0.08626700893551933
+ pos_y: 1.30003226762626
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.6755911323171522
+ q_w: 0.7372764894762539
+ rpy: 0.000000 -0.000000 1.483532
+ wings: 6
\ No newline at end of file
diff --git a/src/mtc/results/644645394.yaml b/src/mtc/results/644645394.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e9ff5ef7a35868d85d78d2a784c12e3ee7b1877f
--- /dev/null
+++ b/src/mtc/results/644645394.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 6
\ No newline at end of file
diff --git a/src/mtc/results/646524863.yaml b/src/mtc/results/646524863.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0e99341436353f2527ed1ae7c66507e5380fa50f
--- /dev/null
+++ b/src/mtc/results/646524863.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.2000020444393158
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361815618200975
+ q_w: 0.9990482753357227
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 7
+Robot_arm2:
+ pos_x: -0.08626700893551933
+ pos_y: 1.30003226762626
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.6755911323171522
+ q_w: 0.7372764894762539
+ rpy: 0.000000 -0.000000 1.483532
+ wings: 7
\ No newline at end of file
diff --git a/src/mtc/results/654874922.yaml b/src/mtc/results/654874922.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..eeeb1919558d8befc646b6e78ef0cbe09f074118
--- /dev/null
+++ b/src/mtc/results/654874922.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.1000020429491997
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -0.1000053047009202
+ pos_y: 1.40499790757487
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 7
\ No newline at end of file
diff --git a/src/mtc/results/678892590.yaml b/src/mtc/results/678892590.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c2fabed44a030761594c7e5e18f113b799c0779f
--- /dev/null
+++ b/src/mtc/results/678892590.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.2000020444393158
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.2000053061910363
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/703393863.yaml b/src/mtc/results/703393863.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f58992998312f304972e596bc9fea567a3bc8c13
--- /dev/null
+++ b/src/mtc/results/703393863.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.2000020444393158
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.2000053061910363
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 6
\ No newline at end of file
diff --git a/src/mtc/results/713515827.yaml b/src/mtc/results/713515827.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3a5f1f23d9ce659b617df58e69e83f88e936863e
--- /dev/null
+++ b/src/mtc/results/713515827.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.2000020444393158
+ pos_y: -2.041459083557129e-06
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 7
+Robot_arm2:
+ pos_x: -0.2000053061910363
+ pos_y: 1.304997906084754
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.707107680321073
+ q_w: 0.7071058820508792
+ rpy: 0.000000 -0.000000 1.570799
+ wings: 7
\ No newline at end of file
diff --git a/src/mtc/results/837930359.yaml b/src/mtc/results/837930359.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d5d94c867206ddd95e632c8040a527fdac592a23
--- /dev/null
+++ b/src/mtc/results/837930359.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.2000020444393158
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361815618200975
+ q_w: 0.9990482753357227
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 7
+Robot_arm2:
+ pos_x: -0.08626700893551933
+ pos_y: 1.400032269116376
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361806884243188
+ q_w: 0.9990482791489393
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 3
\ No newline at end of file
diff --git a/src/mtc/results/866060299.yaml b/src/mtc/results/866060299.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a18207672fc1e7125ab63cfb45529132eb87374f
--- /dev/null
+++ b/src/mtc/results/866060299.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.2000020444393158
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361815618200975
+ q_w: 0.9990482753357227
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 7
+Robot_arm2:
+ pos_x: -0.08626700893551933
+ pos_y: 1.400032269116376
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.6755911323171522
+ q_w: 0.7372764894762539
+ rpy: 0.000000 -0.000000 1.483532
+ wings: 6
\ No newline at end of file
diff --git a/src/mtc/results/875997872.yaml b/src/mtc/results/875997872.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..29297e1d2c6c941c50ed002d33a8d52ebfcb1e20
--- /dev/null
+++ b/src/mtc/results/875997872.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -0.2000020444393158
+ pos_y: 0.09999796003103256
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: -0.04361815618200975
+ q_w: 0.9990482753357227
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 7
+Robot_arm2:
+ pos_x: -0.08626700893551933
+ pos_y: 1.400032269116376
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 0.6755911323171522
+ q_w: 0.7372764894762539
+ rpy: 0.000000 -0.000000 1.483532
+ wings: 7
\ No newline at end of file
diff --git a/src/mtc/results/1353136708.yaml b/src/mtc/results/973613550.yaml
similarity index 100%
rename from src/mtc/results/1353136708.yaml
rename to src/mtc/results/973613550.yaml
diff --git a/src/mtc/results/981928342.yaml b/src/mtc/results/981928342.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..001a1c9ae7b89aba14e15f874890a5623729aac9
--- /dev/null
+++ b/src/mtc/results/981928342.yaml
@@ -0,0 +1,20 @@
+Robot_arm1:
+ pos_x: -2.041459083557129e-06
+ pos_y: -0.2000020444393158
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.249713353108504e-06
+ q_w: 0.9999999999992193
+ rpy: 0.000000 -0.000000 0.000002
+ wings: 5
+Robot_arm2:
+ pos_x: -5.303210804067631e-06
+ pos_y: 1.104997903104521
+ pos_z: 0.4424999952316284
+ q_x: 0
+ q_y: 0
+ q_z: 1.337136133234621e-06
+ q_w: 0.9999999999991062
+ rpy: 0.000000 -0.000000 0.000003
+ wings: 1
\ No newline at end of file
diff --git a/src/mtc/results/1377345014.yaml b/src/mtc/results/991845382.yaml
similarity index 100%
rename from src/mtc/results/1377345014.yaml
rename to src/mtc/results/991845382.yaml
diff --git a/src/mtc/src/base_routine.cpp b/src/mtc/src/base_routine.cpp
index 025a2b4860ed12c2065f41799d23e6508deef8a1..d6536f3ecdba2f195e4089cbddaaf7c8fe061d46 100644
--- a/src/mtc/src/base_routine.cpp
+++ b/src/mtc/src/base_routine.cpp
@@ -36,6 +36,10 @@ int main(int argc, char **argv){
Abstract_mediator* mediator = new Mediator(map_loader->task_grasps(), new ros::Publisher(n.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1)));
mediator->set_result_vector(results);
+ map_loader->write_task();
+
+ /*
+
visualization_msgs::MarkerArray* wings1 = new visualization_msgs::MarkerArray();
visualization_msgs::MarkerArray* wings2 = new visualization_msgs::MarkerArray();
visualization_msgs::MarkerArray* fields1 = new visualization_msgs::MarkerArray();
@@ -59,6 +63,7 @@ int main(int argc, char **argv){
mediator->connect_robots(robo2);
mediator->mediate();
+ */
/*
for (int i = 0; i < ceti->access_fields1().size(); i++){
@@ -74,9 +79,9 @@ int main(int argc, char **argv){
free(map_loader);
free(strategy);
- free(robo);
+ //free(robo);
- free(robo2);
+ //free(robo2);
return 0;
}
\ No newline at end of file
diff --git a/src/mtc/src/cell_routine.cpp b/src/mtc/src/cell_routine.cpp
index 66e7d10e9a86d5eef5d23ab0a7d38d4a33fbd87b..c8d41faa627bd7b7ebd98b663b27eb8373837afa 100644
--- a/src/mtc/src/cell_routine.cpp
+++ b/src/mtc/src/cell_routine.cpp
@@ -6,6 +6,7 @@
#include "impl/wing_moveit_decorator.h"
#include "impl/wing.h"
#include "impl/moveit_robot.h"
+#include "impl/collision_helper.h"
#include <xmlrpcpp/XmlRpc.h>
int main(int argc, char **argv){
@@ -72,6 +73,7 @@ int main(int argc, char **argv){
ceti1->notify();
ceti2->notify();
moveit_mediator->publish_tables();
+ moveit_mediator->check_collision(1);
diff --git a/src/mtc/src/impl/collision_helper.cpp b/src/mtc/src/impl/collision_helper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..66cc07240873fc3980e8c04d84af7d19e4333669
--- /dev/null
+++ b/src/mtc/src/impl/collision_helper.cpp
@@ -0,0 +1,31 @@
+#include "impl/collision_helper.h"
+#include <moveit/collision_detection/collision_common.h>
+#include <fcl/broadphase/broadphase_dynamic_AABB_tree.h>
+
+bool Collision_helper::collision_detector(const std::string& object1_name, const std::string& object2_name){
+ for (std::map<std::string, collision_detection::FCLObject>::const_iterator it = fcl_objs_.begin(); it != fcl_objs_.end(); ++it){
+ ROS_INFO("object %s", it->first);
+ }
+ /*
+ auto objA = fcl_objs_.at(object1_name);
+ auto objB = fcl_objs_.at(object2_name);
+
+
+
+ auto manager1 = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
+ auto manager2 = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
+
+ objA.registerTo(manager1.get());
+ objB.registerTo(manager2.get());
+
+ // Perform collision detection
+ collision_detection::CollisionRequest req;
+ collision_detection::CollisionResult res;
+ collision_detection::CollisionData cd;
+ cd.req_ = &req;
+ cd.res_ = &res;
+ manager1->collide(manager2.get(), &cd, &collision_detection::collisionCallback);
+ */
+ //ROS_INFO("läuft %i", fcl_objs_.size());
+ return true;
+}
\ No newline at end of file
diff --git a/src/mtc/src/impl/map_loader.cpp b/src/mtc/src/impl/map_loader.cpp
index 2c84d2f0f12f37994a6493caf11cae7b5cd0b705..3c2ae07417531c2188089dbd4f437e051bbe142f 100644
--- a/src/mtc/src/impl/map_loader.cpp
+++ b/src/mtc/src/impl/map_loader.cpp
@@ -137,3 +137,251 @@ std::vector<pcl::PointXYZ> Abstract_map_loader::create_pcl_box(){
return box;
}
+void Map_loader::write_task(){
+
+ std::ofstream o(ros::package::getPath("mtc") + "/mtc_task_file/" + std::to_string(static_cast<int>(ros::Time::now().toNSec())) + ".yaml");
+ tf2::Transform target_start = task_grasps_[0].front();
+ tf2::Transform target_end = task_grasps_[0].back();
+ YAML::Node node;
+ //YAML::Comment("yaml-language-server: $schema=/home/matteo/reachability/src/yaml_to_mtc/config/yaml_to_mtc_schema.json");
+ YAML::Node planner_node;
+ planner_node["id"] = "cartesian";
+ planner_node["type"] = "CartesianPath";
+ node["planners"].push_back(planner_node);
+ planner_node.reset();
+
+ planner_node["id"] = "sampling";
+ planner_node["type"] = "PipelinePlanner";
+ planner_node["properties"]["step_size"] = 0.005f;
+ planner_node["properties"]["goal_joint_tolerance"] = 0.00001;
+ node["planners"].push_back(planner_node);
+ planner_node.reset();
+
+ planner_node["id"] = "interpolation";
+ planner_node["type"] = "JointInterpolationPlanner";
+ node["planners"].push_back(planner_node);
+ planner_node.reset();
+
+
+ node["task"]["name"] = "Pick and Place test";
+ node["task"]["properties"]["group"] = "Robot_arm1";
+ node["task"]["properties"]["eef"] = "hand_1";
+ node["task"]["properties"]["hand_grasping_frame"] = "panda_1_link8";
+ node["task"]["properties"]["ik_frame"] = "panda_1_link8";
+ node["task"]["properties"]["hand"] = "hand_1";
+
+ YAML::Node stage;
+ stage["name"] = "current";
+ stage["type"] = "CurrentState";
+ node["task"]["stages"].push_back(stage);
+ stage.reset();
+
+ stage["name"] = "move to ready";
+ stage["type"] = "MoveTo";
+ stage["id"] = "ready";
+ stage["planner"] = "sampling";
+ stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
+ stage["propertiesConfigureInitFrom"]["values"].push_back("group");
+ stage["set"]["goal"] = "ready";
+ node["task"]["stages"].push_back(stage);
+ stage.reset();
+
+ stage["type"] = "MoveTo";
+ stage["planner"] = "sampling";
+ stage["id"] = "hand_open";
+ stage["properties"]["group"] = "hand_1";
+ stage["set"]["goal"] = "open";
+ node["task"]["stages"].push_back(stage);
+ stage.reset();
+
+ stage["type"] = "Connect";
+ stage["group_planner_vector"]["panda_arm"] = "sampling";
+ stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
+ node["task"]["stages"].push_back(stage);
+ stage.reset();
+
+ stage["type"] = "SerialContainer";
+ stage["name"] = "grasp";
+ stage["properties_exposeTo"]["source"] = "task";
+ stage["properties_exposeTo"]["values"] = YAML::Load("[eef, hand, group, ik_frame]");
+ stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
+ stage["propertiesConfigureInitFrom"]["values"] = YAML::Load("[eef, hand, group, ik_frame]");
+ //node["task"]["stages"].push_back(stage);
+
+ YAML::Node stage_in_stage;
+ stage_in_stage["type"] = "MoveRelative";
+ stage_in_stage["planner"] = "cartesian";
+ stage_in_stage["properties"]["link"] = "panda_1_link8";
+ stage_in_stage["properties"]["min_distance"] = 0.07f;
+ stage_in_stage["properties"]["max_distance"] = 0.2f;
+ stage_in_stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
+ stage_in_stage["propertiesConfigureInitFrom"]["values"] = YAML::Load("[group]");
+ stage_in_stage["set"]["direction"]["vector"] = YAML::Load("{x: 0.0, y: 0.0, z: 1.0, header: { frame_id: panda_1_link8 }}");
+ stage["stages"].push_back(stage_in_stage);
+ stage_in_stage.reset();
+
+ stage_in_stage["type"] = "ComputeIK";
+ stage_in_stage["properties"] = YAML::Load("{max_ik_solutions: 5}");
+ stage_in_stage["set"]["ik_frame"]["isometry"]["translation"] = YAML::Load("{ x: 0.1, y: 0.0, z: 0.0 }");
+ stage_in_stage["set"]["ik_frame"]["isometry"]["quaternion"] = YAML::Load("{ r: 1.571, p: 0.785, y: 1.571 }");
+ stage_in_stage["set"]["ik_frame"]["link"] = "panda_1_link8";
+
+ YAML::Node properties_in_stage_in_stage;
+ properties_in_stage_in_stage["source"]= "PARENT";
+ properties_in_stage_in_stage["values"]= YAML::Load("[eef, group]");
+ stage_in_stage["propertiesConfigureInitFrom"].push_back(properties_in_stage_in_stage);
+ properties_in_stage_in_stage.reset();
+
+ properties_in_stage_in_stage["source"]= "INTERFACE";
+ properties_in_stage_in_stage["values"]= YAML::Load("[target_pose]");
+ stage_in_stage["propertiesConfigureInitFrom"].push_back(properties_in_stage_in_stage);
+ properties_in_stage_in_stage.reset();
+
+ stage_in_stage["stage"]["type"] = "GenerateGraspPose";
+ stage_in_stage["stage"]["propertiesConfigureInitFrom"]["source"] = "PARENT";
+ stage_in_stage["stage"]["properties"]["object"] = "bottle/body";
+ stage_in_stage["stage"]["properties"]["angle_delta"] = 1.571f;
+ stage_in_stage["stage"]["properties"]["pregrasp"] = "open";
+ stage_in_stage["stage"]["set"]["monitored_stage"] = "ready";
+ stage["stages"].push_back(stage_in_stage);
+ stage_in_stage.reset();
+
+ stage_in_stage["type"] = "ModifyPlanningScene";
+ stage_in_stage["set"]["allow_collisions"]["first"] = "bottle";
+ stage_in_stage["set"]["allow_collisions"]["second"]["joint_model_group_name"] = "hand_1";
+ stage_in_stage["set"]["allow_collisions"]["allow"] = true;
+ stage["stages"].push_back(stage_in_stage);
+ stage_in_stage.reset();
+
+ stage_in_stage["type"] = "MoveTo";
+ stage_in_stage["planner"] = "sampling";
+ stage_in_stage["properties"]["group"] = "hand_1";
+ stage_in_stage["set"]["goal"] = "close";
+ stage["stages"].push_back(stage_in_stage);
+ stage_in_stage.reset();
+
+ stage_in_stage["type"] = "ModifyPlanningScene";
+ stage_in_stage["set"]["attach_object"]["object"] = "bottle";
+ stage_in_stage["set"]["attach_object"]["link"] = "panda_1_link8";
+ stage["stages"].push_back(stage_in_stage);
+ stage_in_stage.reset();
+
+ stage_in_stage["type"] = "MoveRelative";
+ stage_in_stage["planner"] = "cartesian";
+ stage_in_stage["id"] = "pick_up";
+ stage_in_stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
+ stage_in_stage["propertiesConfigureInitFrom"]["values"] = YAML::Load("[group]");
+ stage_in_stage["properties"]["min_distance"] = 0.1f;
+ stage_in_stage["properties"]["max_distance"] = 0.2f;
+ stage_in_stage["set"]["ik_frame"]["link"] = "panda_1_link8";
+ stage_in_stage["set"]["direction"]["vector"] = YAML::Load("{ x: 0.0, y: 0.0, z: 1.0 }");
+ stage["stages"].push_back(stage_in_stage);
+ stage_in_stage.reset();
+ node["task"]["stages"].push_back(stage);
+
+ stage.reset();
+
+ stage["type"] = "Connect";
+ stage["group_planner_vector"]["panda_arm"] = "sampling";
+ stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
+ node["task"]["stages"].push_back(stage);
+ stage.reset();
+
+ // satrtet here
+ stage["type"] = "SerialContainer";
+ stage["name"] = "place";
+ stage["properties_exposeTo"]["source"] = "task";
+ stage["properties_exposeTo"]["values"] = YAML::Load("[eef, hand, group, ik_frame]");
+ stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
+
+ stage_in_stage["type"] = "MoveRelative";
+ stage_in_stage["planner"] = "cartesian";
+ stage_in_stage["properties"]["link"] = "panda_1_link8";
+ stage_in_stage["properties"]["min_distance"] = 0.1f;
+ stage_in_stage["properties"]["max_distance"] = 0.2f;
+ stage_in_stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
+ stage_in_stage["propertiesConfigureInitFrom"]["values"] = YAML::Load("[group]");
+ stage_in_stage["set"]["direction"]["vector"] = YAML::Load("{ x: 0.0, y: 0.0, z: -1.0}");
+ stage["stages"].push_back(stage_in_stage);
+ stage_in_stage.reset();
+
+ stage_in_stage["type"] = "ComputeIK";
+ stage_in_stage["properties"] = YAML::Load("{ max_ik_solutions: 5 }");
+ stage_in_stage["set"]["ik_frame"]["isometry"]["translation"] = YAML::Load("{ x: 0.1, y: 0.0, z: 0.0 }");
+ stage_in_stage["set"]["ik_frame"]["isometry"]["quaternion"] = YAML::Load("{ r: 1.571, p: 0.785, y: 1.571 }");
+ stage_in_stage["set"]["ik_frame"]["link"] = "panda_1_link8";
+
+ properties_in_stage_in_stage["source"]= "PARENT";
+ properties_in_stage_in_stage["values"]= YAML::Load("[eef, group]");
+ stage_in_stage["propertiesConfigureInitFrom"].push_back(properties_in_stage_in_stage);
+ properties_in_stage_in_stage.reset();
+
+ properties_in_stage_in_stage["source"]= "INTERFACE";
+ properties_in_stage_in_stage["values"]= YAML::Load("[target_pose]");
+ stage_in_stage["propertiesConfigureInitFrom"].push_back(properties_in_stage_in_stage);
+ properties_in_stage_in_stage.reset();
+
+ stage_in_stage["stage"]["type"] = "GeneratePose";
+ stage_in_stage["stage"]["set"]["monitored_stage"] = "pick_up";
+ stage_in_stage["stage"]["set"]["pose"]["point"] = YAML::Load("{ x: 0.4f, y: 0.5f, z: 0.1 }"); // Hier objekt
+ stage_in_stage["stage"]["set"]["pose"]["orientation"] = YAML::Load("{ x: 0.0, y: 0.0, z: 0.0, w: 1.0}");
+ stage["stages"].push_back(stage_in_stage);
+ stage_in_stage.reset();
+
+ stage_in_stage["type"] = "MoveTo";
+ stage_in_stage["planner"] = "sampling";
+ stage_in_stage["properties"]["group"] = "hand_1";
+ stage_in_stage["set"]["goal"] = "open";
+ stage["stages"].push_back(stage_in_stage);
+ stage_in_stage.reset();
+
+ stage_in_stage["type"] = "ModifyPlanningScene";
+ stage_in_stage["set"]["detach_object"]["object"]= "bottle";
+ stage_in_stage["set"]["detach_object"]["link"]= "panda_1_link8";
+ stage_in_stage["set"]["allow_collisions"]["first"]= "bottle";
+ stage_in_stage["set"]["allow_collisions"]["second"]["joint_model_group_name"]= "hand_1";
+ stage_in_stage["set"]["allow_collisions"]["allow"] = false;
+ stage["stages"].push_back(stage_in_stage);
+ stage_in_stage.reset();
+
+ stage_in_stage["type"] = "MoveRelative";
+ stage_in_stage["planner"] = "cartesian";
+ stage_in_stage["properties"]["link"] = "panda_1_link8";
+ stage_in_stage["properties"]["min_distance"] = 0.07f;
+ stage_in_stage["properties"]["max_distance"] = 0.2f;
+ stage_in_stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
+ stage_in_stage["propertiesConfigureInitFrom"]["values"] = YAML::Load("[group]");
+ stage_in_stage["set"]["direction"]["vector"] = YAML::Load("{x: 0.0, y: 0.0, z: -1.0, header: { frame_id: panda_1_link8 }}");
+ stage["stages"].push_back(stage_in_stage);
+ stage_in_stage.reset();
+
+ stage_in_stage["type"] = "MoveTo";
+ stage_in_stage["planner"] = "sampling";
+ stage_in_stage["properties"]["group"] = "hand_1";
+ stage_in_stage["set"]["goal"] = "close";
+ stage["stages"].push_back(stage_in_stage);
+ stage_in_stage.reset();
+
+ stage_in_stage["name"] = "move to ready";
+ stage_in_stage["type"] = "MoveTo";
+ stage_in_stage["planner"] = "sampling";
+ stage_in_stage["propertiesConfigureInitFrom"]["source"]= "PARENT";
+ stage_in_stage["propertiesConfigureInitFrom"]["value"].push_back("group");
+ stage_in_stage["set"]["goal"] = "ready";
+ stage["stages"].push_back(stage_in_stage);
+ stage_in_stage.reset();
+ node["task"]["stages"].push_back(stage);
+ stage.reset();
+
+ node["max_planning_solutions"] = 10;
+
+
+
+
+
+
+ o << node;
+ o.close();
+
+}
+
diff --git a/src/mtc/src/impl/mediator.cpp b/src/mtc/src/impl/mediator.cpp
index 6e10a129346cb2e37bc176c99703e6d32a56bc47..37f089d089712f0938cd42111498197ad0ff87e0 100644
--- a/src/mtc/src/impl/mediator.cpp
+++ b/src/mtc/src/impl/mediator.cpp
@@ -66,6 +66,7 @@ void Mediator::mediate(){
if(objects_[0][i].getOrigin().distance(objects_[1].back().getOrigin()) == 0) objects_[1].pop_back();
}
+
ros::Rate loop_rate(10);
Robot* ceti1 = dynamic_cast<Robot*>(robots_[0]);
for(int j = 0; j <= 7; j++){
diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp
index 7fc1153eac7f88bbad62150cef227cd9a2745c7b..dbb98eac7c1d6b280f8ad914332f24336f8e70c1 100644
--- a/src/mtc/src/impl/moveit_mediator.cpp
+++ b/src/mtc/src/impl/moveit_mediator.cpp
@@ -1,5 +1,8 @@
#include "impl/moveit_mediator.h"
#include "impl/wing_moveit_decorator.h"
+#include "impl/collision_helper.h"
+#include <yaml_to_mtc.h>
+
void Moveit_mediator::publish_tables(){
@@ -27,8 +30,28 @@ void Moveit_mediator::load_robot_description(){
ps_ = new planning_scene::PlanningScene(kinematic_model_);
}
-bool Moveit_mediator::check_collision(const int& robot){return true;};
-void Moveit_mediator::mediate(){};
+bool Moveit_mediator::check_collision(const int& robot){
+ auto env = ps_->getCollisionEnv();
+ Collision_helper* ch = static_cast<Collision_helper*>(const_cast<collision_detection::CollisionEnv*>(env.get()));
+ return ch->collision_detector("object_A", "object_B"); //
+}
+
+void Moveit_mediator::mediate(){
+ /*Yaml_Mtc_Parser parser = Yaml_Mtc_Parser();
+ moveit::task_constructor::Task task = parser.init_task(this->nh_);
+ int max_planning_solutions = 1; // default one solution
+ this->nh_.getParam("max_planning_solutions", max_planning_solutions);
+ try {
+ if (task.plan(max_planning_solutions)) {
+ task.introspection().publishSolution(*task.solutions().front());
+ }
+ } catch (const moveit::task_constructor::InitStageException& ex) {
+ std::cerr << "planning failed with exception" << std::endl << ex << task;
+ }
+
+ ros::waitForShutdown();
+ */
+};
void Moveit_mediator::build_wings(std::bitset<3>& wing, Robot* robot){
std::bitset<3> result = robot->observer_mask() & wing;
diff --git a/src/mtc/src/impl/robot.cpp b/src/mtc/src/impl/robot.cpp
index ae3fe828fe8c5a384060585340f90215fd4f975c..cb7c9db5119f541b8914f66ebb075834ffeb58d7 100644
--- a/src/mtc/src/impl/robot.cpp
+++ b/src/mtc/src/impl/robot.cpp
@@ -21,6 +21,11 @@ void Robot::generate_access_fields(){
};
};
+ access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1.305f,0,0))));
+ access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-1.305f,0,0))));
+ access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,1.305f,0))));
+ access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-1.305f,0))));
+
}
float Robot::area_calculation(tf2::Transform& A, tf2::Transform& B, tf2::Transform& C){
diff --git a/src/panda_moveit_config/CMakeLists.txt b/src/panda_moveit_config/CMakeLists.txt
index 43e77c5c2a0f8726547938192f29f5f10379eef9..b4e880c67048e6eadc6a48a994880593a9903906 100644
--- a/src/panda_moveit_config/CMakeLists.txt
+++ b/src/panda_moveit_config/CMakeLists.txt
@@ -3,7 +3,6 @@ project(panda_moveit_config)
find_package(catkin REQUIRED)
-
catkin_package()
install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
diff --git a/src/panda_moveit_config/config/panda_arm_hand.srdf.xacro b/src/panda_moveit_config/config/panda_arm_hand.srdf.xacro
index ad4ca68b3bfecac1703c60f77abf7234d9ba2b85..c2c065cbae96790c1f948d4dd1b4698716690df1 100644
--- a/src/panda_moveit_config/config/panda_arm_hand.srdf.xacro
+++ b/src/panda_moveit_config/config/panda_arm_hand.srdf.xacro
@@ -35,7 +35,7 @@
<joint name="panda_finger_joint2" value="0" />
</group_state>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
- <end_effector name="hand" parent_link="panda_link8" group="hand" parent_group="panda_arm" />
+ <end_effector name="hand" parent_link="panda_link8" group="hand" parent_group="panda_arm_hand" />
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="panda_hand" link2="panda_link3" reason="Never" />
<disable_collisions link1="panda_hand" link2="panda_link4" reason="Never" />
diff --git a/src/panda_moveit_config/launch/demo.launch b/src/panda_moveit_config/launch/demo.launch
index 76eaf751dc89d4a701060d983030413cb15840dc..2ebb63b0d5a6c82fc573bb05ffcf62e5c66de0b5 100644
--- a/src/panda_moveit_config/launch/demo.launch
+++ b/src/panda_moveit_config/launch/demo.launch
@@ -5,12 +5,9 @@
<!-- Allow user to specify database location -->
<arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db" />
- <arg name="GOAL"/>
-
<!-- By default, we are not in debug mode -->
<arg name="debug" default="false" />
<arg name="pipeline" default="ompl" />
- <arg name="use_rviz" default="true" />
<!--
By default, hide joint_state_publisher's GUI
@@ -26,7 +23,6 @@
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
<include file="$(find panda_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
- <arg name="GOAL" value="$(arg GOAL)"/>
</include>
<!-- If needed, broadcast static tf for robot root -->
@@ -49,12 +45,11 @@
<arg name="info" value="true"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="pipeline" value="$(arg pipeline)" />
- <arg name="GOAL" value="$(arg GOAL)"/>
</include>
- <!-- Run Rviz and load the default config to see the state of the move_group node -->
- <include file="$(find dual_panda_moveit_config)/launch/moveit_rviz.launch" if="$(arg use_rviz)">
- <arg name="rviz_config" value="$(find dual_panda_moveit_config)/launch/moveit.rviz"/>
+ <!-- Run Rviz -->
+ <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch">
+ <arg name="rviz_tutorial" value="$(arg rviz_tutorial)"/>
<arg name="debug" value="$(arg debug)"/>
</include>
diff --git a/src/panda_moveit_config/launch/move_group.launch b/src/panda_moveit_config/launch/move_group.launch
index 36f1794aa9e04eb8caee7c7a555bf856de7dc5e3..f5f9e7ddd7e9dcd2ce49310db846d3ba3e191af6 100644
--- a/src/panda_moveit_config/launch/move_group.launch
+++ b/src/panda_moveit_config/launch/move_group.launch
@@ -1,12 +1,9 @@
<launch>
<arg name="load_gripper" default="true" />
- <arg name="GOAL" />
-
<include file="$(find panda_moveit_config)/launch/planning_context.launch">
<arg name="load_gripper" value="$(arg load_gripper)" />
- <arg name="GOAL" value="$(arg GOAL)" />
</include>
<arg name="pipeline" default="ompl" />
diff --git a/src/panda_moveit_config/launch/moveit.rviz b/src/panda_moveit_config/launch/moveit.rviz
index 76165072ec0309cc8160eb429b65d1d364f9c5f1..38875b8dd8a9291b259b23fe3dece3fdfab0002a 100644
--- a/src/panda_moveit_config/launch/moveit.rviz
+++ b/src/panda_moveit_config/launch/moveit.rviz
@@ -139,7 +139,7 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Loop Animation: true
- Robot Alpha: 0.5
+ Robot Alpha: 1.0
Robot Color: 150; 50; 150
Show Robot Collision: false
Show Robot Visual: true
diff --git a/src/panda_moveit_config/launch/planning_context.launch b/src/panda_moveit_config/launch/planning_context.launch
index df4daea49a3376f24a72c82cb928bbc08bd84d10..f0ec470e19a41ec4899068296a6bb8faa6a2ad06 100644
--- a/src/panda_moveit_config/launch/planning_context.launch
+++ b/src/panda_moveit_config/launch/planning_context.launch
@@ -1,7 +1,5 @@
<launch>
<arg name="load_gripper" default="true" />
- <arg name="GOAL" />
-
<!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
<arg name="load_robot_description" default="false"/>
@@ -14,7 +12,7 @@
<param if="$(eval arg('load_robot_description') and not arg('load_gripper'))" name="$(arg robot_description)" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm.urdf.xacro'"/>
<!-- The semantic description that corresponds to the URDF -->
- <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda_arm_hand.srdf.xacro' 'GOAL:=$(arg GOAL)'" if="$(arg load_gripper)" />
+ <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda_arm_hand.srdf.xacro'" if="$(arg load_gripper)" />
<param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda_arm.srdf.xacro'" unless="$(arg load_gripper)" />
<!-- Load updated joint limits (override information from URDF) -->