diff --git a/.catkin_tools/profiles/default/build.yaml b/.catkin_tools/profiles/default/build.yaml
index 122835594a26db730f6f4ac3160d4daf5d186ce0..910ef428350b2f977ff36bed501d87b46cb53802 100644
--- a/.catkin_tools/profiles/default/build.yaml
+++ b/.catkin_tools/profiles/default/build.yaml
@@ -10,8 +10,7 @@ extends: null
install: false
install_space: install
isolate_install: false
-jobs_args:
-- -j2
+jobs_args: []
licenses:
- TODO
log_space: logs
diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 78c3c1b805dec2b7496ff8f136ae97520bd8b3ec..f0fa9d9d117da6367d04dd3636ec84b944d8264c 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 11015
+/home/matteo/reachability/devel/./cmake.lock 11297
/home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
diff --git a/src/franka_description/robots/ceti_double.urdf.xacro b/src/franka_description/robots/ceti_double.urdf.xacro
index 7a82590bd5e38830a449ec784b16eb35d8390c62..b3dd77ef1a68d3efb4e4b71988eb475e12ffd11d 100644
--- a/src/franka_description/robots/ceti_double.urdf.xacro
+++ b/src/franka_description/robots/ceti_double.urdf.xacro
@@ -10,25 +10,40 @@
<link name="world" />
+ <xacro:property name="yaml_file" value="$(find mtc)/results/-668288260.yaml" />
+ <xacro:property name="props" value="${xacro.load_yaml(yaml_file)}" />
+
+ <xacro:property name="x1" value="${props['Robot_arm1']['pos_x']}" />
+ <xacro:property name="y1" value="${props['Robot_arm1']['pos_y']}" />
+ <xacro:property name="z1" value="${props['Robot_arm1']['pos_z']}" />
+ <xacro:property name="rpy1" value="${props['Robot_arm1']['rpy']}" />
+
+
+ <xacro:property name="x2" value="${props['Robot_arm2']['pos_x']}" />
+ <xacro:property name="y2" value="${props['Robot_arm2']['pos_y']}" />
+ <xacro:property name="z2" value="${props['Robot_arm2']['pos_z']}" />
+ <xacro:property name="rpy2" value="${props['Robot_arm2']['rpy']}" />
+
+
<!-- Dummy Link -->
<joint name="base_joint1" type="fixed">
<parent link="world" />
<child link="base_1" />
- <origin xyz="0 0 0" rpy="0 0 0"/>
+ <origin xyz="${x1} ${y1} ${z1}" rpy="${rpy1}"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="base_joint2" type="fixed">
<parent link="world" />
<child link="base_2" />
- <origin xyz="0 0 0" rpy="0 0 0"/>
+ <origin xyz="${x2} ${y2} ${z2}" rpy="${rpy2}"/>
<axis xyz="0 0 1"/>
</joint>
<link name="base_1">
<visual>
- <origin xyz="-1 0 0.4425" rpy="0 0 0"/>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.8 0.8 0.885" />
</geometry>
@@ -37,7 +52,7 @@
</material>
</visual>
<collision>
- <origin xyz="-1 0 0.4425" rpy="0 0 0"/>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.8 0.8 0.885" />
</geometry>
@@ -46,7 +61,7 @@
<link name="base_2">
<visual>
- <origin xyz="1 0 0.4425" rpy="0 0 0"/>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.8 0.8 0.885" />
</geometry>
@@ -55,7 +70,7 @@
</material>
</visual>
<collision>
- <origin xyz="1 0 0.4425" rpy="0 0 0"/>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.8 0.8 0.885" />
</geometry>
@@ -64,11 +79,11 @@
<!-- right arm with gripper -->
- <xacro:panda_arm arm_id="$(arg arm_id_1)" connected_to="base_1" xyz="-1.22 0 0.885" />
+ <xacro:panda_arm arm_id="$(arg arm_id_1)" connected_to="base_1" xyz="-0.22 0 ${z1}" />
<xacro:hand ns="$(arg arm_id_1)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_1)_link8" />
<!-- left arm with gripper -->
- <xacro:panda_arm arm_id="$(arg arm_id_2)" connected_to="base_2" xyz="0.78 0 0.885" />
+ <xacro:panda_arm arm_id="$(arg arm_id_2)" connected_to="base_2" xyz="-0.22 0 ${z2}" />
<xacro:hand ns="$(arg arm_id_2)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_2)_link8"/>
</robot>
diff --git a/src/mtc/descriptions/dummy2.yaml b/src/mtc/descriptions/dummy2.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5fe8bea2718de6fd2c9092bb5b0aac0025525f78
--- /dev/null
+++ b/src/mtc/descriptions/dummy2.yaml
@@ -0,0 +1 @@
+{task: {groups: {robot_x: [-0.300000 -0.700000 0.890000, -0.300000 -0.600000 0.890000, -0.200000 -0.700000 0.890000, -0.200000 -0.600000 0.890000, 0.100000 -0.700000 0.890000, 0.100000 -0.600000 0.890000, 0.100000 -0.300000 0.890000, 0.100000 -0.200000 0.890000, 0.100000 -0.100000 0.890000, 0.200000 -0.300000 0.890000, 0.200000 -0.200000 0.890000, 0.200000 -0.100000 0.890000, 0.300000 -0.300000 0.890000, 0.300000 -0.200000 0.890000, 0.300000 -0.100000 0.890000, 0.100000 0.100000 0.890000, 0.100000 0.200000 0.890000, 0.100000 0.300000 0.890000, 0.200000 0.100000 0.890000, 0.200000 0.200000 0.890000, 0.200000 0.300000 0.890000, 0.300000 0.100000 0.890000, 0.300000 0.200000 0.890000, 0.300000 0.300000 0.890000, -0.300000 0.600000 0.890000, -0.300000 0.700000 0.890000, -0.200000 0.600000 0.890000, -0.200000 0.700000 0.890000, 0.100000 0.600000 0.890000, 0.100000 0.700000 0.890000, -0.300000 0.600000 0.890000, -0.300000 0.700000 0.890000, -0.200000 0.600000 0.890000, -0.200000 0.700000 0.890000, 0.100000 0.600000 0.890000, 0.100000 0.700000 0.890000], robot_y: [0.100000 1.010000 0.890000, 0.100000 1.110000 0.890000, 0.100000 1.210000 0.890000, 0.200000 1.010000 0.890000, 0.200000 1.110000 0.890000, 0.200000 1.210000 0.890000, 0.300000 1.010000 0.890000, 0.300000 1.110000 0.890000, 0.300000 1.210000 0.890000, 0.100000 1.410000 0.890000, 0.100000 1.510000 0.890000, 0.100000 1.610000 0.890000, 0.200000 1.410000 0.890000, 0.200000 1.510000 0.890000, 0.200000 1.610000 0.890000, 0.300000 1.410000 0.890000, 0.300000 1.510000 0.890000, 0.300000 1.610000 0.890000, -0.300000 1.910000 0.890000, -0.300000 2.010000 0.890000, -0.200000 1.910000 0.890000, -0.200000 2.010000 0.890000, 0.100000 1.910000 0.890000, 0.100000 2.010000 0.890000, -0.300000 0.600000 0.890000, -0.300000 0.700000 0.890000, -0.200000 0.600000 0.890000, -0.200000 0.700000 0.890000, 0.100000 0.600000 0.890000, 0.100000 0.700000 0.890000, -0.300000 0.600000 0.890000, -0.300000 0.700000 0.890000, -0.200000 0.600000 0.890000, -0.200000 0.700000 0.890000, 0.100000 0.600000 0.890000, 0.100000 0.700000 0.890000]}}}
\ No newline at end of file
diff --git a/src/mtc/include/impl/abstract_mediator.h b/src/mtc/include/impl/abstract_mediator.h
index 247274582b22e329e50cde91d47dbd1c6b9bcfe5..d9a161b0cafb770de5395abd005ebfbdac0f138c 100644
--- a/src/mtc/include/impl/abstract_mediator.h
+++ b/src/mtc/include/impl/abstract_mediator.h
@@ -1,12 +1,16 @@
#ifndef ABSTRACT_MEDIATOR_
#define ABSTRACT_MEDIATOR_
-#include "ros/ros.h"
+#include <ros/ros.h>
#include "impl/abstract_robot.h"
#include "impl/abstract_robot_element.h"
#include "impl/robot.h"
-#include "octomap/octomap.h"
+#include <ros/package.h>
+#include <yaml-cpp/yaml.h>
+#include <fstream>
+
+#include <octomap/octomap.h>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
@@ -38,8 +42,8 @@ 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,-0.6525f,0.4425f -0.005f)), left));
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));
}
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 9b41c207260bec0bd70b76fbf448a59cde68ab8a..d8a2823e7a2a0e12e2b2493776ed5610a1d8ca04 100644
--- a/src/mtc/include/impl/abstract_robot.h
+++ b/src/mtc/include/impl/abstract_robot.h
@@ -54,6 +54,7 @@ class Abstract_robot {
inline std::string& name() { return name_;}
inline tf2::Transform& tf() { return tf_;}
inline tf2::Transform& root_tf() { return root_tf_;}
+ inline std::vector<tf2::Transform>& bounds() {return bounds_;}
inline void set_tf(tf2::Transform& t) { tf_ = t;}
inline std::vector<tf2::Transform>& robot_root_bounds() { return robot_root_bounds_;}
inline void rotate(float deg) {tf2::Quaternion rot; rot.setRPY(0,0,deg); tf2::Transform t(rot, tf2::Vector3(0,0,0)); tf_= tf_* t;}
diff --git a/src/mtc/include/impl/mediator.h b/src/mtc/include/impl/mediator.h
index c1389289e7dc1482293d26627f8629b6d7e446f3..700e6bfbbf3f4c3e24f4ebb989134aca7f02c28e 100644
--- a/src/mtc/include/impl/mediator.h
+++ b/src/mtc/include/impl/mediator.h
@@ -18,8 +18,9 @@ class Mediator : public Abstract_mediator{
pcl::PointCloud< pcl::PointXYZ >::Ptr vector_to_cloud(std::vector<pcl::PointXYZ>& vector);
std::vector<pcl::PointXYZ> generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution);
- void approximation(Robot* robot, int& wings, std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>>& dict);
- void write_file(std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>>& dict);
+ void approximation(Robot* robot, int& wings);
+ void write_file(Robot* A, int& a, Robot* B, int& b);
+
bool check_collision(const int& robot) override;
void mediate() override;
diff --git a/src/mtc/include/impl/robot.h b/src/mtc/include/impl/robot.h
index 1a51167293167190d195c28ebed3ad899461dbea..a7cc77bd61411c8b0e840f96c08772e2bf95e564 100644
--- a/src/mtc/include/impl/robot.h
+++ b/src/mtc/include/impl/robot.h
@@ -41,6 +41,7 @@ class Robot : public Abstract_robot{
void reset();
void generate_access_fields();
float area_calculation(tf2::Transform& A, tf2::Transform& B, tf2::Transform& C);
+ bool check_robot_collision(Robot* R);
bool check_single_object_collision(tf2::Transform& obj) override;
void notify() override;
diff --git a/src/mtc/include/impl/wing.h b/src/mtc/include/impl/wing.h
index cc44f795ad6a3127b747b0e173998c672911e8c5..15f2cefa3fc5ca884375887e3c5f927c8157ec13 100644
--- a/src/mtc/include/impl/wing.h
+++ b/src/mtc/include/impl/wing.h
@@ -18,7 +18,7 @@ class Wing : public Abstract_robot_element{
bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/-2.f, size_.getY()/2.f,0))); //-+
}
inline tf2::Vector3 size(){ return size_;}
- inline std::vector<tf2::Transform> bounds() {return bounds_;}
+ inline std::vector<tf2::Transform>& bounds() {return bounds_;}
void update(tf2::Transform& tf) override {this->calc_world_tf(tf);}
};
diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch
index d394fd582456818ddba30e976e7927fbd34243b3..4999cebde4b7275a4608a030f4ee87139554beb0 100644
--- a/src/mtc/launch/cell_routine.launch
+++ b/src/mtc/launch/cell_routine.launch
@@ -1,5 +1,7 @@
<launch>
<!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
+ <rosparam command="load" file="$(find mtc)/results/-668288260.yaml"/>
+
<include file="$(find ceti_double)/launch/demo.launch">
</include>
diff --git a/src/mtc/results/-1770475312.yaml b/src/mtc/results/-1770475312.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..08bf979d3f806e91a5fd56e8295e16490539ca64
--- /dev/null
+++ b/src/mtc/results/-1770475312.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: -6.040541873092198e-06
+ pos_y: 1.699997983867894
+ 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: 7
\ No newline at end of file
diff --git a/src/mtc/results/-1777977373.yaml b/src/mtc/results/-1777977373.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7f16b008b1fb5cf01305af5a4c84ccf7bbdc45e9
--- /dev/null
+++ b/src/mtc/results/-1777977373.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: -6.040541873092198e-06
+ pos_y: 1.699997983867894
+ 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: 6
\ No newline at end of file
diff --git a/src/mtc/results/-1786268055.yaml b/src/mtc/results/-1786268055.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2db387fdc81ac9fc2aab80605cf2ea55fcb359f3
--- /dev/null
+++ b/src/mtc/results/-1786268055.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: -6.040541873092198e-06
+ pos_y: 1.699997983867894
+ 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: 5
\ No newline at end of file
diff --git a/src/mtc/results/-1796122988.yaml b/src/mtc/results/-1796122988.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..bc7ccf28e86d61d45b3675587e80d25ee7246865
--- /dev/null
+++ b/src/mtc/results/-1796122988.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: -6.040541873092198e-06
+ pos_y: 1.699997983867894
+ 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: 4
\ No newline at end of file
diff --git a/src/mtc/results/-2009327429.yaml b/src/mtc/results/-2009327429.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1ee1e171f42eea43180ea9193d77a7f2ebf9023e
--- /dev/null
+++ b/src/mtc/results/-2009327429.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: -6.040541873092198e-06
+ pos_y: 1.599997982377778
+ 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: 7
\ No newline at end of file
diff --git a/src/mtc/results/-2016725910.yaml b/src/mtc/results/-2016725910.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..df4a7a9eb88fdb641f095b1854e4258c7d10d3be
--- /dev/null
+++ b/src/mtc/results/-2016725910.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: -6.040541873092198e-06
+ pos_y: 1.599997982377778
+ 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: 6
\ No newline at end of file
diff --git a/src/mtc/results/-2023885311.yaml b/src/mtc/results/-2023885311.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5070c55ae3065a9c787c0f4adb77e185e780982f
--- /dev/null
+++ b/src/mtc/results/-2023885311.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: -6.040541873092198e-06
+ pos_y: 1.599997982377778
+ 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: 5
\ No newline at end of file
diff --git a/src/mtc/results/-2030540640.yaml b/src/mtc/results/-2030540640.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e9d498887f339bc0a7ac5ed5480abe956298e53f
--- /dev/null
+++ b/src/mtc/results/-2030540640.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: -6.040541873092198e-06
+ pos_y: 1.599997982377778
+ 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: 4
\ No newline at end of file
diff --git a/src/mtc/results/1115810607.yaml b/src/mtc/results/1115810607.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..361796e053ad1500c89c16ad81982b478312c594
--- /dev/null
+++ b/src/mtc/results/1115810607.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: -0.04361815618200975
+ q_w: 0.9990482753357227
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 7
+Robot_arm2:
+ pos_x: 0.03944321899097195
+ pos_y: 1.593909842936157
+ 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: 5
\ No newline at end of file
diff --git a/src/mtc/results/1130176764.yaml b/src/mtc/results/1130176764.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a62e54f61d1e7b823a08d5a909df518f9951dc76
--- /dev/null
+++ b/src/mtc/results/1130176764.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: -0.04361815618200975
+ q_w: 0.9990482753357227
+ rpy: 0.000000 0.000000 -0.087264
+ wings: 7
+Robot_arm2:
+ pos_x: 0.03944321899097195
+ pos_y: 1.593909842936157
+ 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: 7
\ No newline at end of file
diff --git a/src/mtc/results/1353136708.yaml b/src/mtc/results/1353136708.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a2073f42f3990bac768f6ceda98bd0d05a4405a0
--- /dev/null
+++ b/src/mtc/results/1353136708.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: -6.040541873092198e-06
+ pos_y: 1.399997979397545
+ 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/1377345014.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..843a661189018947fecafbd6397577d009f822ba
--- /dev/null
+++ b/src/mtc/results/1377345014.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: -6.040541873092198e-06
+ pos_y: 1.399997979397545
+ 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/1392124514.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..cc5deeab9e3ccb963682ac0693dde94aa823bbc3
--- /dev/null
+++ b/src/mtc/results/1392124514.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: -6.040541873092198e-06
+ pos_y: 1.399997979397545
+ 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: 5
\ No newline at end of file
diff --git a/src/mtc/results/1407514948.yaml b/src/mtc/results/1407514948.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0213fb7994e3b3ae00547db7200b1c11a32a3aca
--- /dev/null
+++ b/src/mtc/results/1407514948.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: -6.040541873092198e-06
+ pos_y: 1.399997979397545
+ 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: 7
\ No newline at end of file
diff --git a/src/mtc/results/1434971863.yaml b/src/mtc/results/1434971863.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..fd493a07e3a6a626cfbfa76d06697ee868e00404
--- /dev/null
+++ b/src/mtc/results/1434971863.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.03944321899097195
+ pos_y: 1.693909844426273
+ 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: 4
\ No newline at end of file
diff --git a/src/mtc/results/1441695710.yaml b/src/mtc/results/1441695710.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ca12d20c1e9954210ebf56c8f202a14069a3fff9
--- /dev/null
+++ b/src/mtc/results/1441695710.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.03944321899097195
+ pos_y: 1.693909844426273
+ 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: 5
\ No newline at end of file
diff --git a/src/mtc/results/1448889126.yaml b/src/mtc/results/1448889126.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..170c9fa0eba351e6bc832906c6a2d6ffbc3e2107
--- /dev/null
+++ b/src/mtc/results/1448889126.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.03944321899097195
+ pos_y: 1.693909844426273
+ 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: 6
\ No newline at end of file
diff --git a/src/mtc/results/1456397962.yaml b/src/mtc/results/1456397962.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c2e968ec4eb0df9712e8fcc9fe0c9cff961b9ed1
--- /dev/null
+++ b/src/mtc/results/1456397962.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.03944321899097195
+ pos_y: 1.693909844426273
+ 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: 7
\ No newline at end of file
diff --git a/src/mtc/results/1569464609.yaml b/src/mtc/results/1569464609.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6811dbd06958de6f84823ae78e2dbb7f3fd2c70b
--- /dev/null
+++ b/src/mtc/results/1569464609.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: -6.040541873092198e-06
+ pos_y: 1.499997980887662
+ 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: 5
\ No newline at end of file
diff --git a/src/mtc/results/1584757889.yaml b/src/mtc/results/1584757889.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..71aa4f21e33cc102e329ba611d0393762266ed49
--- /dev/null
+++ b/src/mtc/results/1584757889.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: -6.040541873092198e-06
+ pos_y: 1.499997980887662
+ 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: 7
\ No newline at end of file
diff --git a/src/mtc/results/1794683721.yaml b/src/mtc/results/1794683721.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..032b2b6032c72213fe710230ab0350d72dfff5f5
--- /dev/null
+++ b/src/mtc/results/1794683721.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: -6.040541873092198e-06
+ pos_y: 1.599997982377778
+ 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: 4
\ No newline at end of file
diff --git a/src/mtc/results/1801771460.yaml b/src/mtc/results/1801771460.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1ea80fa9741bf321b4362c396b512e09ac4d1681
--- /dev/null
+++ b/src/mtc/results/1801771460.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: -6.040541873092198e-06
+ pos_y: 1.599997982377778
+ 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: 5
\ No newline at end of file
diff --git a/src/mtc/results/1809636855.yaml b/src/mtc/results/1809636855.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..182cef8999770779c10d6f38be2a132f410e0e2d
--- /dev/null
+++ b/src/mtc/results/1809636855.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: -6.040541873092198e-06
+ pos_y: 1.599997982377778
+ 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: 6
\ No newline at end of file
diff --git a/src/mtc/results/1817405794.yaml b/src/mtc/results/1817405794.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7bc2377d2efb7a783ccf4a3bdad949cb9905b725
--- /dev/null
+++ b/src/mtc/results/1817405794.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: -6.040541873092198e-06
+ pos_y: 1.599997982377778
+ 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: 7
\ No newline at end of file
diff --git a/src/mtc/results/1859157926.yaml b/src/mtc/results/1859157926.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b15d6f45bba5181bf18de7aebac3e3454ca7ec48
--- /dev/null
+++ b/src/mtc/results/1859157926.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: -6.040541873092198e-06
+ pos_y: 1.399997979397545
+ 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/1871941323.yaml b/src/mtc/results/1871941323.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0f43af5a3fa2e279512c72dbe45978d5f54b05c9
--- /dev/null
+++ b/src/mtc/results/1871941323.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: -6.040541873092198e-06
+ pos_y: 1.399997979397545
+ 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/1885436442.yaml b/src/mtc/results/1885436442.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4c54e786372ab5ce023437633296c72f8abd9f04
--- /dev/null
+++ b/src/mtc/results/1885436442.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: -6.040541873092198e-06
+ pos_y: 1.399997979397545
+ 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: 5
\ No newline at end of file
diff --git a/src/mtc/results/1899539074.yaml b/src/mtc/results/1899539074.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4f724d7ef5cb00d1fd4b09b6558c4fe6e5b40db4
--- /dev/null
+++ b/src/mtc/results/1899539074.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: -6.040541873092198e-06
+ pos_y: 1.399997979397545
+ 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: 7
\ No newline at end of file
diff --git a/src/mtc/results/2029203334.yaml b/src/mtc/results/2029203334.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0e5d4f4c3d54458bc61c392e8012f871eeb51505
--- /dev/null
+++ b/src/mtc/results/2029203334.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: -6.040541873092198e-06
+ pos_y: 1.699997983867894
+ 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: 4
\ No newline at end of file
diff --git a/src/mtc/results/2036675920.yaml b/src/mtc/results/2036675920.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..814f6a684cddc639b9de10f0e880243fffd1be05
--- /dev/null
+++ b/src/mtc/results/2036675920.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: -6.040541873092198e-06
+ pos_y: 1.699997983867894
+ 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: 5
\ No newline at end of file
diff --git a/src/mtc/results/2045443294.yaml b/src/mtc/results/2045443294.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0957e6e0f299966b038356d0bdc1c8d49ffc21a6
--- /dev/null
+++ b/src/mtc/results/2045443294.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: -6.040541873092198e-06
+ pos_y: 1.699997983867894
+ 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: 6
\ No newline at end of file
diff --git a/src/mtc/results/2051106760.yaml b/src/mtc/results/2051106760.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..17781de1f30e80f9aae1a9f2c7e8fa0f8fdb5520
--- /dev/null
+++ b/src/mtc/results/2051106760.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: -6.040541873092198e-06
+ pos_y: 1.499997980887662
+ 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: 5
\ No newline at end of file
diff --git a/src/mtc/results/2053540854.yaml b/src/mtc/results/2053540854.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0dd2c0dfd06783fa89f63b9f38d3f2e2ee08ae57
--- /dev/null
+++ b/src/mtc/results/2053540854.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: -6.040541873092198e-06
+ pos_y: 1.699997983867894
+ 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: 7
\ No newline at end of file
diff --git a/src/mtc/results/2065375902.yaml b/src/mtc/results/2065375902.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..57ed50f0f0d34178fdd7348d7dd97b9884089628
--- /dev/null
+++ b/src/mtc/results/2065375902.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: -6.040541873092198e-06
+ pos_y: 1.499997980887662
+ 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: 7
\ No newline at end of file
diff --git a/src/mtc/src/base_routine.cpp b/src/mtc/src/base_routine.cpp
index 532c2c16d2aadf449f455fc18116be38ab2ca08a..025a2b4860ed12c2065f41799d23e6508deef8a1 100644
--- a/src/mtc/src/base_routine.cpp
+++ b/src/mtc/src/base_routine.cpp
@@ -52,12 +52,14 @@ int main(int argc, char **argv){
Abstract_robot* robo2 = new Robot(std::string("Robot_arm2"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f)));
Robot* ceti2 = dynamic_cast<Robot*>(robo2);
+ ceti2->set_observer_mask(wing_config::RML);
ceti2->add_rviz_markers(wings2);
ceti2->add_rviz_markers(fields2);
mediator->connect_robots(robo2);
mediator->mediate();
+
/*
for (int i = 0; i < ceti->access_fields1().size(); i++){
ceti->access_fields1()[i] = new Field_rviz_decorator(ceti->access_fields1()[i], fields1);
diff --git a/src/mtc/src/cell_routine.cpp b/src/mtc/src/cell_routine.cpp
index 752f3d6d9f19cb8acf08895a703ba27345142361..66e7d10e9a86d5eef5d23ab0a7d38d4a33fbd87b 100644
--- a/src/mtc/src/cell_routine.cpp
+++ b/src/mtc/src/cell_routine.cpp
@@ -13,6 +13,28 @@ int main(int argc, char **argv){
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
+
+ float x1,y1,z1,qx1, qy1, qz1, qw1;
+ float x2,y2,z2,qx2, qy2, qz2, qw2;
+ int a,b;
+ nh.getParam("/Robot_arm1/pos_x",x1);
+ nh.getParam("/Robot_arm1/pos_y",y1);
+ nh.getParam("/Robot_arm1/pos_z",z1);
+ nh.getParam("/Robot_arm1/q_x",qx1);
+ nh.getParam("/Robot_arm1/q_y",qy1);
+ nh.getParam("/Robot_arm1/q_z",qz1);
+ nh.getParam("/Robot_arm1/q_w",qw1);
+ nh.getParam("/Robot_arm1/wings",a);
+
+ nh.getParam("/Robot_arm2/pos_x",x2);
+ nh.getParam("/Robot_arm2/pos_y",y2);
+ nh.getParam("/Robot_arm2/pos_z",z2);
+ nh.getParam("/Robot_arm2/q_x",qx2);
+ nh.getParam("/Robot_arm2/q_y",qy2);
+ nh.getParam("/Robot_arm2/q_z",qz2);
+ nh.getParam("/Robot_arm2/q_w",qw2);
+ nh.getParam("/Robot_arm2/wings",b);
+
std::vector<std::vector<tf2::Transform>> objects; //in progress
ros::Publisher* pub = new ros::Publisher(nh.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1)); //refractor
@@ -22,8 +44,12 @@ int main(int argc, char **argv){
moveit_mediator->load_robot_description();
- Abstract_robot* robo1 = new Moveit_robot(std::string("panda_arm1"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,0,0.4425f)));
- Abstract_robot* robo2 = new Moveit_robot(std::string("panda_arm2"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-1,0,0.4425f)));
+ ROS_INFO("%f %f %f %f %f %f %f", x1,y1,z1,qx1, qy1, qz1, qw1);
+ ROS_INFO("%f %f %f %f %f %f %f", x2,y2,z2,qx2, qy2, qz2, qw2);
+
+
+ Abstract_robot* robo1 = new Moveit_robot(std::string("panda_arm1"), tf2::Transform(tf2::Quaternion(qx1,qy1,qz1,qw1), tf2::Vector3(x1,y1,0.4425f)));
+ Abstract_robot* robo2 = new Moveit_robot(std::string("panda_arm2"), tf2::Transform(tf2::Quaternion(qx2,qy2,qz2,qw2), tf2::Vector3(x2,y2,0.4425f)));
robo1->set_observer_mask(wing_config::RML);
robo2->set_observer_mask(wing_config::RML);
@@ -38,9 +64,10 @@ int main(int argc, char **argv){
Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(robo2);
- std::bitset<3> i = 7;
- moveit_mediator->build_wings(i, ceti1);
- moveit_mediator->build_wings(i, ceti2);
+ std::bitset<3> ia = a;
+ std::bitset<3> ib = b;
+ moveit_mediator->build_wings(ia, ceti1);
+ moveit_mediator->build_wings(ib, ceti2);
ceti1->notify();
ceti2->notify();
diff --git a/src/mtc/src/impl/base_by_rotation.cpp b/src/mtc/src/impl/base_by_rotation.cpp
index f16328cf5d95ac94ae97fa517e9cbce4a07a76cf..df8d314214db66442b4aab6240d672a5867b410e 100644
--- a/src/mtc/src/impl/base_by_rotation.cpp
+++ b/src/mtc/src/impl/base_by_rotation.cpp
@@ -5,10 +5,10 @@ void Base_by_rotation::inv_map_creation(Abstract_map_loader* var) {
ROS_INFO("starting base calculation strategy...");
ROS_INFO("condition based [inv_map] calculation...");
std::vector<tf2::Transform> trans;
- for(int i = 0; i < var->map().size(); i++) {
- for (int x = 0; x < var->target_rot().size(); x++) {
- for (int y = 0; y < var->target_rot()[x].size(); y++) {
- for(int p = 0; p < var->target_rot()[x][y].size(); p++){
+ for(long unsigned int i = 0; i < var->map().size(); i++) {
+ for (long unsigned int x = 0; x < var->target_rot().size(); x++) {
+ for (long unsigned int y = 0; y < var->target_rot()[x].size(); y++) {
+ for(long unsigned int p = 0; p < var->target_rot()[x][y].size(); p++){
if (var->target_rot()[x][y][p].angle(var->map()[i].getRotation()) < 0.349066f) {
trans.push_back(tf2::Transform(var->target_rot()[x][y][p], var->map()[i].getOrigin()).inverse());
break;
@@ -18,7 +18,7 @@ void Base_by_rotation::inv_map_creation(Abstract_map_loader* var) {
}
}
var->set_inv_map(trans);
- ROS_INFO("caculated [inv_map] contains %i entrys...", var->inv_map().size());
+ ROS_INFO("caculated [inv_map] contains %li entrys...", var->inv_map().size());
};
void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) {
@@ -27,8 +27,8 @@ void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) {
// Optimize targets in later implementation
std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>> target_clouds;
target_clouds.resize(var->task_grasps().size());
- for(int i = 0; i < target_clouds.size(); i++) {
- for (int j = 0; j < var->task_grasps()[i].size();j++) {
+ for(long unsigned int i = 0; i < target_clouds.size(); i++) {
+ for (long unsigned int j = 0; j < var->task_grasps()[i].size();j++) {
target_clouds[i].push_back(pcl::PointCloud< pcl::PointXYZ >::Ptr(new pcl::PointCloud< pcl::PointXYZ >));
}
}
@@ -36,10 +36,10 @@ void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) {
// Maybe OpenMP
ROS_INFO("start [cloud] calculation...");
tf2::Transform root(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f));
- for (int i = 0; i < target_clouds.size(); i++){
- for (int j = 0; j < target_clouds[i].size(); j++) {
- for (int y = 0; y < var->inv_map().size(); y++) {
- for (int x = 0; x < var->target_rot()[i][j].size(); x++) {
+ for (long unsigned int i = 0; i < target_clouds.size(); i++){
+ for (long unsigned int j = 0; j < target_clouds[i].size(); j++) {
+ for (long unsigned int y = 0; y < var->inv_map().size(); y++) {
+ for (long unsigned int x = 0; x < var->target_rot()[i][j].size(); x++) {
tf2::Transform target = var->task_grasps()[i][j]; target.setRotation(var->target_rot()[i][j][x]);
tf2::Transform base = target * (var->inv_map()[y] * root);
target_clouds[i][j]->push_back(pcl::PointXYZ(base.getOrigin().getX(), base.getOrigin().getY(), base.getOrigin().getZ()));
diff --git a/src/mtc/src/impl/map_loader.cpp b/src/mtc/src/impl/map_loader.cpp
index 785247636fb9e9b7ad21923a4523f5c5c6a589a8..2c84d2f0f12f37994a6493caf11cae7b5cd0b705 100644
--- a/src/mtc/src/impl/map_loader.cpp
+++ b/src/mtc/src/impl/map_loader.cpp
@@ -17,7 +17,7 @@ Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){
_map.push_back(tf2::Transform(so.normalize(),t));
}
this->set_map(_map);
- ROS_INFO("[map] saved with %i entries...", _map.size());
+ ROS_INFO("[map] saved with %li entries...", _map.size());
ROS_INFO("saving [target] positions...");
std::vector<std::vector<tf2::Transform>> task_;
@@ -33,7 +33,7 @@ Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){
}
this->set_task_grasps(task_);
- for (int i = 0; i < task_.size(); i++) ROS_INFO("[target] for Robot %i saved with %i entries...", i, task_[i].size());
+ for (long unsigned int i = 0; i < task_.size(); i++) ROS_INFO("[target] for Robot %li saved with %li entries...", i, task_[i].size());
}
std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){
@@ -50,9 +50,9 @@ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){
basic_rot.push_back(z_rot.inverse().normalize());
std::vector<std::vector<std::vector<tf2::Quaternion>>> target_orientation_grasps;
- for (int i = 0; i < task_grasps_.size(); i++) {
+ for (long unsigned int i = 0; i < task_grasps_.size(); i++) {
std::vector<std::vector<tf2::Quaternion>> quat;
- for (int j = 0; j < task_grasps_[i].size(); j++) {
+ for (long unsigned int j = 0; j < task_grasps_[i].size(); j++) {
quat.push_back(basic_rot);
}
target_orientation_grasps.push_back(quat);
@@ -66,21 +66,21 @@ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){
std::vector<pcl::PointXYZ> voxelization = this->create_pcl_box();
std::vector<std::vector<std::vector<int>>> base_target_map;
base_target_map.resize(task_grasps_.size());
- for(int i = 0; i < base_target_map.size();i++) base_target_map[i].resize(voxelization.size());
+ for(long unsigned int i = 0; i < base_target_map.size();i++) base_target_map[i].resize(voxelization.size());
ROS_INFO("forming base clouds...");
strategy_->cloud_calculation(this);
// OpenMP
ROS_INFO("start cloud quantization...");
- for(int i = 0; i < target_cloud_.size();i++){
- for(int j = 0; j < target_cloud_[i].size();j++){
+ for(long unsigned int i = 0; i < target_cloud_.size();i++){
+ for(long unsigned int j = 0; j < target_cloud_[i].size();j++){
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.2f);
octree.setInputCloud(target_cloud_[i][j]);
octree.addPointsFromInputCloud();
double min_x, min_y, min_z, max_x, max_y, max_z;
octree.getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
- for(int k = 0; k < voxelization.size(); k++) {
+ for(long unsigned int k = 0; k < voxelization.size(); k++) {
pcl::PointXYZ p = voxelization[k];
bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);
if(isInBox && octree.isVoxelOccupiedAtPoint(p)) {
@@ -96,7 +96,7 @@ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){
std::vector<std::vector<pcl::PointXYZ>> resulting;
- for(int i = 0; i < base_target_map.size(); i++) {
+ for(long unsigned int i = 0; i < base_target_map.size(); i++) {
std::vector<pcl::PointXYZ> points_per_robot;
for(int j = 0; j < base_target_map[i].size(); j++){
if (base_target_map[i][j].size() == task_grasps_[i].size()) {
@@ -106,8 +106,8 @@ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){
if (!points_per_robot.empty()) resulting.push_back(points_per_robot);
}
- for (int i = 0; i < resulting.size(); i++) {
- ROS_INFO("Robot %i got %i base positions to ckeck", i, resulting[i].size());
+ for (long unsigned int i = 0; i < resulting.size(); i++) {
+ ROS_INFO("Robot %li got %li base positions to ckeck", i, resulting[i].size());
}
return resulting;
}
diff --git a/src/mtc/src/impl/mediator.cpp b/src/mtc/src/impl/mediator.cpp
index 60f09ad52946e5ef95eaeade84120384afb49f00..6e10a129346cb2e37bc176c99703e6d32a56bc47 100644
--- a/src/mtc/src/impl/mediator.cpp
+++ b/src/mtc/src/impl/mediator.cpp
@@ -1,12 +1,13 @@
#include "impl/mediator.h"
#include "impl/wing_rviz_decorator.h"
+#include <tf2/LinearMath/Scalar.h>
bool Mediator::check_collision( const int& robot){
bool succ = true;
- for(int j = 0; j < objects_[robot].size(); j++){
+ for(long unsigned int j = 0; j < objects_[robot].size(); j++){
visualization_msgs::Marker marker;
marker.header.frame_id = "map";
marker.header.stamp = ros::Time();
@@ -39,9 +40,9 @@ bool Mediator::check_collision( const int& robot){
rviz_markers_->markers.push_back(marker);
}
return succ;
-
}
+
void Mediator::mediate(){
ROS_INFO("assigne result to first robot...");
std::vector<pcl::PointXYZ> grCenter = Abstract_mediator::generate_Ground(tf2::Vector3(0,0,0), 3.0f, 0.1f);
@@ -61,121 +62,114 @@ void Mediator::mediate(){
}
}
- for (int i = objects_[0].size()-1; i > 0; i--){
+ for (long unsigned int i = objects_[0].size()-1; i > 0; i--){
if(objects_[0][i].getOrigin().distance(objects_[1].back().getOrigin()) == 0) objects_[1].pop_back();
}
- std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>> dict;
-
-
ros::Rate loop_rate(10);
Robot* ceti1 = dynamic_cast<Robot*>(robots_[0]);
-
for(int j = 0; j <= 7; j++){
std::bitset<3> wing_config(j);
build_wings(wing_config, ceti1);
-
- for (int i = 0; i < ground_per_robot.size(); i++){
+
+ for (long unsigned int i = 0; i < ground_per_robot.size(); i++){
robots_[0]->set_tf(ground_per_robot[i]);
for ( float p = 0; p < 2*M_PI; p += 0.0872665f){
for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers()) markers->markers.clear();
rviz_markers_->markers.clear();
ceti1->rotate(0.0872665f);
ceti1->notify();
- bool temp = check_collision(0);
- /*
- if (check_collision(0)){
- approximation(ceti1, j, dict);
+
+ if (check_collision(0)) {
+ approximation(ceti1, j);
} else {
continue;
}
- */
- for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers())pub_->publish(*markers);
+ for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers()) pub_->publish(*markers);
pub_->publish(*rviz_markers_);
}
}
ceti1->reset();
}
-
- if (dict.empty()){
- ROS_INFO("unfortunately, no result is found...");
- return;
- } else {
- ROS_INFO("write data to file...");
-
- }
-
}
-void Mediator::approximation(Robot* robot, int& wings, std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>>& dict){
- ros::Rate loop_rate(1);
- std::vector<pcl::PointXYZ> relative_ground;
- pcl::octree::OctreePointCloudSearch< pcl::PointXYZ > cloud(0.4f);
- cloud.setInputCloud(Abstract_mediator::vector_to_cloud(result_vector_[1]));
- cloud.addPointsFromInputCloud();
-
+void Mediator::approximation(Robot* robot, int& wings){
+ ROS_INFO("assigne result to first robot...");
Robot* ceti = dynamic_cast<Robot*>(robots_[1]);
- std::vector<std::pair<tf2::Transform, int>> second_robot_data;
for (int i = 0; i <= 7; i++){
std::bitset<3> wing_config(i);
build_wings(wing_config, ceti);
-
for (Abstract_robot_element* fields : robot->access_fields()) {
ceti->set_tf(fields->world_tf());
for ( float p = 0; p < 2*M_PI; p += M_PI/2){
+ for (visualization_msgs::MarkerArray* markers : robot->rviz_markers()) markers->markers.clear();
+ for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) markers->markers.clear();
+ rviz_markers_->markers.clear();
+
ceti->rotate(M_PI/2);
ceti->notify();
+
+ if (robot->check_robot_collision(ceti)) continue;
+
if (check_collision(1)) {
ROS_INFO("should work");
- second_robot_data.push_back(std::pair<tf2::Transform, int>(ceti->tf(), i));
+ write_file(robot, wings, ceti, i);
} else {
continue;
}
- loop_rate.sleep();
+ for (visualization_msgs::MarkerArray* markers : robot->rviz_markers()) pub_->publish(*markers);
+ for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) pub_->publish(*markers);
+ pub_->publish(*rviz_markers_);
+
}
+
//relative_ground.push_back(pcl::PointXYZ(fields->world_tf().getOrigin().getX(), fields->world_tf().getOrigin().getY(), fields->world_tf().getOrigin().getZ()));
}
ceti->reset();
}
- std::pair<tf2::Transform, int> ceti1(robot->tf(), wings);
- dict.push_back(std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>(ceti1, second_robot_data));
- ceti->reset();
}
-void Mediator::write_file(std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>>& dict){
- /*
-
- std::ofstream o(ros::package::getPath("mtc") + "/results/dummy.yaml");
- YAML::Node node;
-
-
-
-
- for(tf2::Vector3& vec : robot_1){
- node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));
- }
-
- ROS_INFO("writing items for robot2...");
- for(tf2::Vector3& vec : robot_2){
- node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));
- }
-
- ROS_INFO("writing intersection...");
- for(tf2::Vector3& vec : robot_middle){
- node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
- node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
- }
+void Mediator::write_file(Robot* A, int& a, Robot* B, int& b){
+ YAML::Node node;
+ std::ofstream o(ros::package::getPath("mtc") + "/results/" + std::to_string(static_cast<int>(ros::Time::now().toNSec())) + ".yaml");
+ double r,p,y;
+ tf2::Matrix3x3 m(A->tf().getRotation());
+ m.getRPY(r,p,y);
+
+ node[A->name()]["pos_x"] = A->tf().getOrigin().getX();
+ node[A->name()]["pos_y"] = A->tf().getOrigin().getY();
+ node[A->name()]["pos_z"] = A->tf().getOrigin().getZ();
+ node[A->name()]["q_x"] = A->tf().getRotation().getX();
+ node[A->name()]["q_y"] = A->tf().getRotation().getY();
+ node[A->name()]["q_z"] = A->tf().getRotation().getZ();
+ node[A->name()]["q_w"] = A->tf().getRotation().getW();
+ node[A->name()]["rpy"] = std::to_string(r) + " " + std::to_string(p) + " " + std::to_string(y);
+ node[A->name()]["wings"] = a;
+
+ m.setRotation(B->tf().getRotation());
+ m.getRPY(r,p,y);
+
+
+ node[B->name()]["pos_x"] = B->tf().getOrigin().getX();
+ node[B->name()]["pos_y"] = B->tf().getOrigin().getY();
+ node[B->name()]["pos_z"] = B->tf().getOrigin().getZ();
+ node[B->name()]["q_x"] = B->tf().getRotation().getX();
+ node[B->name()]["q_y"] = B->tf().getRotation().getY();
+ node[B->name()]["q_z"] = B->tf().getRotation().getZ();
+ node[B->name()]["q_w"] = B->tf().getRotation().getW();
+ node[B->name()]["rpy"] = std::to_string(r) + " " + std::to_string(p) + " " + std::to_string(y);
+ node[B->name()]["wings"] = b;
o << node;
- */
+ o.close();
+
}
void Mediator::build_wings(std::bitset<3>& wing, Robot* robot){
std::bitset<3> result = robot->observer_mask() & wing;
- for (int i = 0; i < result.size(); i++){
- ROS_INFO( "%i", i);
+ for (std::size_t i = 0; i < result.size(); i++){
if (result[i]){
Abstract_robot_element* moveit_right = new Wing_rviz_decorator(new Wing(wings_[i].pos_, wings_[i].size_), robot->rviz_markers()[0]);
robot->register_observers(moveit_right);
diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp
index 5e344327d44739c22b40a54d927c474771ffcc80..7fc1153eac7f88bbad62150cef227cd9a2745c7b 100644
--- a/src/mtc/src/impl/moveit_mediator.cpp
+++ b/src/mtc/src/impl/moveit_mediator.cpp
@@ -8,7 +8,7 @@ void Moveit_mediator::publish_tables(){
moveit_msgs::PlanningScene planning_scene;
planning_scene.is_diff = true;
- for(int i = 0; i < robots_.size();i++){
+ for(long unsigned int i = 0; i < robots_.size();i++){
Moveit_robot* ceti = dynamic_cast<Moveit_robot*>(robots_[i]);
for (moveit_msgs::CollisionObject* markers : ceti->coll_markers()) {
planning_scene.world.collision_objects.push_back(*markers);
@@ -32,7 +32,7 @@ void Moveit_mediator::mediate(){};
void Moveit_mediator::build_wings(std::bitset<3>& wing, Robot* robot){
std::bitset<3> result = robot->observer_mask() & wing;
- for (int i = 0; i < result.size(); i++){
+ for (std::size_t i = 0; i < result.size(); i++){
if (result[i]){
moveit_msgs::CollisionObject* marker = new moveit_msgs::CollisionObject();
robot->add_coll_markers(marker);
diff --git a/src/mtc/src/impl/robot.cpp b/src/mtc/src/impl/robot.cpp
index 409862028cbb68ddc6bd5493028f237526a070f3..ae3fe828fe8c5a384060585340f90215fd4f975c 100644
--- a/src/mtc/src/impl/robot.cpp
+++ b/src/mtc/src/impl/robot.cpp
@@ -42,18 +42,14 @@ bool Robot::check_single_object_collision(tf2::Transform& obj){
if ((std::floor(sum*100)/100.f) <= full_area) {return false; }
- for (int i = 0; i < bounds_.size(); i++){
- tf2::Transform A = tf_ * bounds_[0];
- tf2::Transform B = tf_ * bounds_[1];
- tf2::Transform C = tf_ * bounds_[2];
- tf2::Transform D = tf_ * bounds_[3];
+ A = tf_ * bounds_[0];
+ B = tf_ * bounds_[1];
+ C = tf_ * bounds_[2];
+ D = tf_ * bounds_[3];
- full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
- sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
-
-
- if ((std::floor(sum*100)/100.f) <= full_area) return true;
- }
+ full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
+ sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
+ if ((std::floor(sum*100)/100.f) <= full_area) return true;
if (!observers_.empty()){
std::vector<Abstract_robot_element*>::const_iterator it = observers_.begin();
@@ -81,15 +77,78 @@ bool Robot::check_single_object_collision(tf2::Transform& obj){
void Robot::reset(){
observers_.clear();
access_fields_.clear();
-
generate_access_fields();
+
tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f));
}
void Robot::notify(){
- for(Abstract_robot_element* wing : observers_) wing->update(tf_); for(Abstract_robot_element* field : access_fields_) field->update(tf_);
+ for(Abstract_robot_element* wing : observers_) wing->update(tf_);
+ for(Abstract_robot_element* field : access_fields_) field->update(tf_);
}
+bool Robot::check_robot_collision( Robot* rob){
+ tf2::Transform A = tf_ * bounds_[0];
+ tf2::Transform B = tf_ * bounds_[1];
+ tf2::Transform C = tf_ * bounds_[2];
+ tf2::Transform D = tf_ * bounds_[3];
+
+ float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
+ float sum = area_calculation(A,rob->tf(),D) + area_calculation(D,rob->tf(),C) + area_calculation(C,rob->tf(),B) + area_calculation(rob->tf(), B, A);
+ if ((std::floor(sum*100)/100.f) <= full_area) return true;
+
+ if (!rob->observers().empty()){
+ std::vector<Abstract_robot_element*>::const_iterator it = rob->observers().begin();
+ while (it != rob->observers().end()) {
+ Abstract_robot_element_decorator *deco = dynamic_cast<Abstract_robot_element_decorator*>(*it);
+ sum = area_calculation(A,deco->wing()->world_tf(),D) + area_calculation(D,deco->wing()->world_tf(),C) + area_calculation(C,deco->wing()->world_tf(),B) + area_calculation(deco->wing()->world_tf(), B, A);
+ if ((std::floor(sum*100)/100.f) <= full_area) {
+ return true;
+ } else {
+ ++it;
+ }
+ }
+ }
+
+ if (!observers_.empty()){
+ std::vector<Abstract_robot_element*>::const_iterator it = observers_.begin();
+ while (it != observers_.end()) {
+ Abstract_robot_element_decorator *deco = dynamic_cast<Abstract_robot_element_decorator*>(*it);
+ Wing* ceti = dynamic_cast<Wing*>(deco->wing());
+ tf2::Transform A = ceti->world_tf() * ceti->bounds()[0];
+ tf2::Transform B = ceti->world_tf() * ceti->bounds()[1];
+ tf2::Transform C = ceti->world_tf() * ceti->bounds()[2];
+ tf2::Transform D = ceti->world_tf() * ceti->bounds()[3];
+
+ full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
+ sum = area_calculation(A,rob->tf(),D) + area_calculation(D,rob->tf(),C) + area_calculation(C,rob->tf(),B) + area_calculation(rob->tf(), B, A);
+ if ((std::floor(sum*100)/100.f) <= full_area) return true;
+
+ if (!rob->observers().empty()){
+ std::vector<Abstract_robot_element*>::const_iterator itt = rob->observers().begin();
+ while (itt != rob->observers().end()) {
+ Abstract_robot_element_decorator *deco = dynamic_cast<Abstract_robot_element_decorator*>(*itt);
+ sum = area_calculation(A,deco->wing()->world_tf(),D) + area_calculation(D,deco->wing()->world_tf(),C) + area_calculation(C,deco->wing()->world_tf(),B) + area_calculation(deco->wing()->world_tf(), B, A);
+ if ((std::floor(sum*100)/100.f) <= full_area) {
+ return true;
+ } else {
+ ++itt;
+ }
+ }
+ }
+
+ if ((std::floor(sum*100)/100.f) <= full_area) {
+ return true;
+ } else {
+ ++it;
+ }
+ }
+ }
+ return false;
+
+}
+
+
void Robot::register_observers(Abstract_robot_element* wd){
Abstract_robot_element_decorator* decorator = dynamic_cast<Abstract_robot_element_decorator*>(wd);
@@ -99,7 +158,7 @@ void Robot::register_observers(Abstract_robot_element* wd){
int index = 0;
if (decorator->wing()->relative_tf().getOrigin().getY()>0){
- for(int i = 0; i < access_fields_.size(); i++){
+ for(long unsigned int i = 0; i < access_fields_.size(); i++){
if ((access_fields_[i]->relative_tf().getOrigin().getY() > 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){
found = true;
index = i;
@@ -108,7 +167,7 @@ void Robot::register_observers(Abstract_robot_element* wd){
}
if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
} else if (decorator->wing()->relative_tf().getOrigin().getY()<0) {
- for(int i = 0; i < access_fields_.size(); i++){
+ for(long unsigned int i = 0; i < access_fields_.size(); i++){
if ((access_fields_[i]->relative_tf().getOrigin().getY() < 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){
found = true;
index = i;
@@ -117,7 +176,7 @@ void Robot::register_observers(Abstract_robot_element* wd){
}
if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
} else {
- for(int i = 0; i < access_fields_.size(); i++){
+ for(long unsigned int i = 0; i < access_fields_.size(); i++){
if (access_fields_[i]->relative_tf().getOrigin().getX() > 0){
found = true;
index = i;
diff --git a/src/mtc/src/impl/wing_moveit_decorator.cpp b/src/mtc/src/impl/wing_moveit_decorator.cpp
index 6943b43df32ac3ff4ac9053742f4ed3edac9df0a..cb6d96d47b777b6abcd1d136a7665b223856eeaa 100644
--- a/src/mtc/src/impl/wing_moveit_decorator.cpp
+++ b/src/mtc/src/impl/wing_moveit_decorator.cpp
@@ -34,4 +34,7 @@ void Wing_moveit_decorator::output_filter() {
markers_->primitive_poses[0].orientation.w = world_quat.getW();
markers_->operation = markers_->ADD;
+
+ ROS_INFO("%f %f %f %f", world_quat.getX(), world_quat.getY(), world_quat.getZ(), world_quat.getW());
+
}
\ No newline at end of file
diff --git a/src/mtc/src/impl/wing_rviz_decorator.cpp b/src/mtc/src/impl/wing_rviz_decorator.cpp
index c21e91c680c03f9f4f85aea42da6b3dc4773a576..234530266a854a5574f2ebd385c3e18f3e89dd26 100644
--- a/src/mtc/src/impl/wing_rviz_decorator.cpp
+++ b/src/mtc/src/impl/wing_rviz_decorator.cpp
@@ -81,7 +81,7 @@ void Wing_rviz_decorator::output_filter() {
bounds.color.a = 1.0;
bounds.pose.orientation.w = 1;
- for (int i = 0; i < wing->bounds().size(); i++){
+ for (long unsigned int i = 0; i < wing->bounds().size(); i++){
tf2::Transform point_posistion = wing->world_tf() * wing->bounds()[i];
geometry_msgs::Point point;
point.x = point_posistion.getOrigin().getX();
diff --git a/src/mtc/src/mtc2taskspace.cpp b/src/mtc/src/mtc2taskspace.cpp
index 835d671d0cd67c79fb6fba8d1b4820570985221b..db30d166f0f06c865db81ac8ed019ff27b4c34fb 100644
--- a/src/mtc/src/mtc2taskspace.cpp
+++ b/src/mtc/src/mtc2taskspace.cpp
@@ -1,5 +1,7 @@
#include "../include/mtc2taskspace.h"
+
+
int main(int argc, char **argv){
ros::init(argc, argv, "mtc2taskspace");
ros::NodeHandle n;
@@ -58,7 +60,7 @@ int main(int argc, char **argv){
// lets make some result data
ROS_INFO("writing file...");
- std::ofstream o(ros::package::getPath("mtc") + "/descriptions/dummy.yaml");
+ std::ofstream o(ros::package::getPath("mtc") + "/descriptions/dummy2.yaml");
YAML::Node node;
ROS_INFO("writing items for robot1...");
@@ -77,6 +79,15 @@ int main(int argc, char **argv){
node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
}
+ ROS_INFO("writing intersection...");
+
+ for(tf2::Vector3& vec : robot_middle){
+ node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
+ node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
+ }
+
+ node.SetStyle(YAML::EmitterStyle::Flow);
+
o << node;
return 0;
}
\ No newline at end of file