From 6f5396df27cd52d3882cce1dea00c69a7940ba93 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Mon, 24 Oct 2022 17:36:42 +0200
Subject: [PATCH] starting docker
---
.catkin_tools/profiles/default/build.yaml | 3 +-
.../profiles/default/devel_collisions.txt | 2 +-
.../robots/ceti_double.urdf.xacro | 31 +++--
src/mtc/descriptions/dummy2.yaml | 1 +
src/mtc/include/impl/abstract_mediator.h | 10 +-
src/mtc/include/impl/abstract_robot.h | 1 +
src/mtc/include/impl/mediator.h | 5 +-
src/mtc/include/impl/robot.h | 1 +
src/mtc/include/impl/wing.h | 2 +-
src/mtc/launch/cell_routine.launch | 2 +
src/mtc/results/-1770475312.yaml | 20 +++
src/mtc/results/-1777977373.yaml | 20 +++
src/mtc/results/-1786268055.yaml | 20 +++
src/mtc/results/-1796122988.yaml | 20 +++
src/mtc/results/-2009327429.yaml | 20 +++
src/mtc/results/-2016725910.yaml | 20 +++
src/mtc/results/-2023885311.yaml | 20 +++
src/mtc/results/-2030540640.yaml | 20 +++
src/mtc/results/1115810607.yaml | 20 +++
src/mtc/results/1130176764.yaml | 20 +++
src/mtc/results/1353136708.yaml | 20 +++
src/mtc/results/1377345014.yaml | 20 +++
src/mtc/results/1392124514.yaml | 20 +++
src/mtc/results/1407514948.yaml | 20 +++
src/mtc/results/1434971863.yaml | 20 +++
src/mtc/results/1441695710.yaml | 20 +++
src/mtc/results/1448889126.yaml | 20 +++
src/mtc/results/1456397962.yaml | 20 +++
src/mtc/results/1569464609.yaml | 20 +++
src/mtc/results/1584757889.yaml | 20 +++
src/mtc/results/1794683721.yaml | 20 +++
src/mtc/results/1801771460.yaml | 20 +++
src/mtc/results/1809636855.yaml | 20 +++
src/mtc/results/1817405794.yaml | 20 +++
src/mtc/results/1859157926.yaml | 20 +++
src/mtc/results/1871941323.yaml | 20 +++
src/mtc/results/1885436442.yaml | 20 +++
src/mtc/results/1899539074.yaml | 20 +++
src/mtc/results/2029203334.yaml | 20 +++
src/mtc/results/2036675920.yaml | 20 +++
src/mtc/results/2045443294.yaml | 20 +++
src/mtc/results/2051106760.yaml | 20 +++
src/mtc/results/2053540854.yaml | 20 +++
src/mtc/results/2065375902.yaml | 20 +++
src/mtc/src/base_routine.cpp | 2 +
src/mtc/src/cell_routine.cpp | 37 +++++-
src/mtc/src/impl/base_by_rotation.cpp | 22 ++--
src/mtc/src/impl/map_loader.cpp | 22 ++--
src/mtc/src/impl/mediator.cpp | 122 +++++++++---------
src/mtc/src/impl/moveit_mediator.cpp | 4 +-
src/mtc/src/impl/robot.cpp | 91 ++++++++++---
src/mtc/src/impl/wing_moveit_decorator.cpp | 3 +
src/mtc/src/impl/wing_rviz_decorator.cpp | 2 +-
src/mtc/src/mtc2taskspace.cpp | 13 +-
54 files changed, 928 insertions(+), 128 deletions(-)
create mode 100644 src/mtc/descriptions/dummy2.yaml
create mode 100644 src/mtc/results/-1770475312.yaml
create mode 100644 src/mtc/results/-1777977373.yaml
create mode 100644 src/mtc/results/-1786268055.yaml
create mode 100644 src/mtc/results/-1796122988.yaml
create mode 100644 src/mtc/results/-2009327429.yaml
create mode 100644 src/mtc/results/-2016725910.yaml
create mode 100644 src/mtc/results/-2023885311.yaml
create mode 100644 src/mtc/results/-2030540640.yaml
create mode 100644 src/mtc/results/1115810607.yaml
create mode 100644 src/mtc/results/1130176764.yaml
create mode 100644 src/mtc/results/1353136708.yaml
create mode 100644 src/mtc/results/1377345014.yaml
create mode 100644 src/mtc/results/1392124514.yaml
create mode 100644 src/mtc/results/1407514948.yaml
create mode 100644 src/mtc/results/1434971863.yaml
create mode 100644 src/mtc/results/1441695710.yaml
create mode 100644 src/mtc/results/1448889126.yaml
create mode 100644 src/mtc/results/1456397962.yaml
create mode 100644 src/mtc/results/1569464609.yaml
create mode 100644 src/mtc/results/1584757889.yaml
create mode 100644 src/mtc/results/1794683721.yaml
create mode 100644 src/mtc/results/1801771460.yaml
create mode 100644 src/mtc/results/1809636855.yaml
create mode 100644 src/mtc/results/1817405794.yaml
create mode 100644 src/mtc/results/1859157926.yaml
create mode 100644 src/mtc/results/1871941323.yaml
create mode 100644 src/mtc/results/1885436442.yaml
create mode 100644 src/mtc/results/1899539074.yaml
create mode 100644 src/mtc/results/2029203334.yaml
create mode 100644 src/mtc/results/2036675920.yaml
create mode 100644 src/mtc/results/2045443294.yaml
create mode 100644 src/mtc/results/2051106760.yaml
create mode 100644 src/mtc/results/2053540854.yaml
create mode 100644 src/mtc/results/2065375902.yaml
diff --git a/.catkin_tools/profiles/default/build.yaml b/.catkin_tools/profiles/default/build.yaml
index 1228355..910ef42 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 78c3c1b..f0fa9d9 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 7a82590..b3dd77e 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 0000000..5fe8bea
--- /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 2472745..d9a161b 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 9b41c20..d8a2823 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 c138928..700e6bf 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 1a51167..a7cc77b 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 cc44f79..15f2cef 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 d394fd5..4999ceb 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 0000000..08bf979
--- /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 0000000..7f16b00
--- /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 0000000..2db387f
--- /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 0000000..bc7ccf2
--- /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 0000000..1ee1e17
--- /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 0000000..df4a7a9
--- /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 0000000..5070c55
--- /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 0000000..e9d4988
--- /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 0000000..361796e
--- /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 0000000..a62e54f
--- /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 0000000..a2073f4
--- /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 0000000..843a661
--- /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 0000000..cc5deea
--- /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 0000000..0213fb7
--- /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 0000000..fd493a0
--- /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 0000000..ca12d20
--- /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 0000000..170c9fa
--- /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 0000000..c2e968e
--- /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 0000000..6811dbd
--- /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 0000000..71aa4f2
--- /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 0000000..032b2b6
--- /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 0000000..1ea80fa
--- /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 0000000..182cef8
--- /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 0000000..7bc2377
--- /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 0000000..b15d6f4
--- /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 0000000..0f43af5
--- /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 0000000..4c54e78
--- /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 0000000..4f724d7
--- /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 0000000..0e5d4f4
--- /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 0000000..814f6a6
--- /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 0000000..0957e6e
--- /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 0000000..17781de
--- /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 0000000..0dd2c0d
--- /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 0000000..57ed50f
--- /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 532c2c1..025a2b4 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 752f3d6..66e7d10 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 f16328c..df8d314 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 7852476..2c84d2f 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 60f09ad..6e10a12 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 5e34432..7fc1153 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 4098620..ae3fe82 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 6943b43..cb6d96d 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 c21e91c..2345302 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 835d671..db30d16 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
--
GitLab