Skip to content
Snippets Groups Projects
Commit 6f5396df authored by KingMaZito's avatar KingMaZito
Browse files

starting docker

parent 264eb6ff
No related branches found
No related tags found
No related merge requests found
Pipeline #14869 failed
Showing
with 241 additions and 17 deletions
......@@ -10,8 +10,7 @@ extends: null
install: false
install_space: install
isolate_install: false
jobs_args:
- -j2
jobs_args: []
licenses:
- TODO
log_space: logs
......
/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
......@@ -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>
{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
#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());}
......
......@@ -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;}
......
......@@ -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;
......
......@@ -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;
......
......@@ -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);}
};
......
<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>
......
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
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
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
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
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
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
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
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
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
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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment