Skip to content
Snippets Groups Projects
Commit 2b1e4ab0 authored by KingMaZito's avatar KingMaZito
Browse files

between MGs and Task Constructors

parent fe0fb0a2
Branches
Tags
No related merge requests found
Show whitespace changes
Inline Side-by-side
include/mediator/mg_mediator.h
+ 31
− 0
View file @ 2b1e4ab0
...@@ -8,18 +8,47 @@ ...@@ -8,18 +8,47 @@
#include <ccf/connection/MqttConnection.h> #include <ccf/connection/MqttConnection.h>
#include <ccf/controller/Controller.h> #include <ccf/controller/Controller.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/stages/current_state.h>
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
#include <moveit/task_constructor/stages/generate_place_pose.h>
#include <moveit/task_constructor/stages/generate_pose.h>
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/predicate_filter.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
#include <moveit/task_constructor/properties.h>
#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/stages/fixed_state.h>
//should be a mix between MG, bahvior tree, moveit task constructor //should be a mix between MG, bahvior tree, moveit task constructor
class MGMediator: public AbstractMediator, virtual Controller{ class MGMediator: public AbstractMediator, virtual Controller{
protected: protected:
std::shared_ptr<moveit::core::RobotModel> robot_model_;
std::vector<std::unique_ptr<Connection>> connections_; std::vector<std::unique_ptr<Connection>> connections_;
std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_; //!< MoveItVisualTools std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_; //!< MoveItVisualTools
std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_; //!< Planningscene monitor std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_; //!< Planningscene monitor
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi_; //!< Move Group Interface of the whole multi-cell
std::shared_ptr<ros::Publisher> planning_scene_diff_publisher_; //!< Publisher to manage PlanningScene diffs
std::shared_ptr<moveit::task_constructor::solvers::PipelinePlanner> sampling_planner_; //!< Moveit task Constructior simple planner
std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_; //!< Moveit task Constructior cartesian planner
std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_; //!< PlanningSceneInteface to manage Scene Objects std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_; //!< PlanningSceneInteface to manage Scene Objects
std::map<std::string, std::vector<uint8_t>> acm_; //!< shared allowed collision matrix between robots std::map<std::string, std::vector<uint8_t>> acm_; //!< shared allowed collision matrix between robots
std::map<std::string, std::vector<uint8_t>> rs_; //!< shared robot state between all robots std::map<std::string, std::vector<uint8_t>> rs_; //!< shared robot state between all robots
std::shared_ptr<planning_scene::PlanningScene> ps_; //!< Shared Planning Scene
std::string referenceRobot_; //!< Reference Robot std::string referenceRobot_; //!< Reference Robot
...@@ -46,6 +75,8 @@ class MGMediator: public AbstractMediator, virtual Controller{ ...@@ -46,6 +75,8 @@ class MGMediator: public AbstractMediator, virtual Controller{
void setPanel() override; void setPanel() override;
void publishTables(); void publishTables();
moveit::task_constructor::Task pickUp(const std::string& obj, const std::string& robotId);
moveit::task_constructor::Task place(const std::string& obj, const std::string& robotId, const std::string& target);
}; };
#endif #endif
\ No newline at end of file
results/dummy/-2045918175/-2045918175.yaml
+ 4
− 2
View file @ 2b1e4ab0
...@@ -20,8 +20,10 @@ ...@@ -20,8 +20,10 @@
{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, { 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, { 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table2_right_panel' , 'pos': { 'x': -0.000007 , 'y': 1.957498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, { 'id': 'table2_right_panel' , 'pos': { 'x': -0.000007 , 'y': 1.957498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, { 'id': 'A', 'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } },
{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.11, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, { 'id': 'ABIN', 'type': 'OBSTACLE', 'pos': { 'x': -0.25, 'y': -0.65, 'z': 0.9355 },'size': { 'length': 0.0330, 'width': 0.066, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } },
{ 'id': 'B', 'type': 'BOX', 'pos': { 'x': -0.3, 'y': 0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'g': 1 } },
{ 'id': 'BBIN', 'type': 'OBSTACLE', 'pos': { 'x': -0.25, 'y': 1.7, 'z': 0.9355 },'size': { 'length': 0.033, 'width': 0.066, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'g': 1 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
]} ]}
\ No newline at end of file
src/mediator/mg_mediator.cpp
+ 504
− 17
View file @ 2b1e4ab0
...@@ -3,7 +3,65 @@ ...@@ -3,7 +3,65 @@
MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh) MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
: AbstractMediator(nh) : AbstractMediator(nh)
, Controller(*nh.get()) , Controller(*nh.get())
{ , sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
, cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
, psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
, mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
, planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1))){
// MQTT HANDLE
addCallback("command/pickup", [&](const std::string& data) {
std::vector<std::pair<std::string, std::string>> pickup_command{std::make_pair("A", "panda_arm1"), std::make_pair("B", "panda_arm2") };
for (const auto& pair: pickup_command) {
moveit::task_constructor::Task mgt = pickUp(pair.first, pair.second);
if(mgt.plan(1)) {
//mgt.introspection().publishSolution(*mgt.solutions().front());
mgt.execute(*mgt.solutions().front());
}
}
});
addCallback("command/place", [&](const std::string& data) {
std::vector<std::tuple<std::string, std::string, std::string>> place_command{std::make_tuple("A", "panda_arm1", "target"), std::make_tuple("B", "panda_arm2", "target") };
for (const auto& tuple: place_command) {
moveit::task_constructor::Task mgt = place(std::get<0>(tuple), std::get<1>(tuple), std::get<2>(tuple));
if(mgt.plan(1)) {
//mgt.introspection().publishSolution(*mgt.solutions().front());
mgt.execute(*mgt.solutions().front());
}
}
});
moveit::task_constructor::Task mgt2 = place("A", "panda_arm1", "dafd");
if(mgt2.plan(1)) {
//mgt.introspection().publishSolution(*mgt.solutions().front());
mgt2.execute(*mgt2.solutions().front());
}
robot_model_loader::RobotModelLoaderPtr robot_model_loader;
robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
robot_model_ = robot_model_loader->getModel();
planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>("world", "/rviz_visual_tools");
visual_tools_->loadMarkerPub();
visual_tools_->waitForMarkerPub();
visual_tools_->loadRemoteControl();
visual_tools_->setPlanningSceneMonitor(planning_scene_monitor_);
visual_tools_->trigger();
sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
// cartesian
cartesian_planner_->setMaxVelocityScaling(1.0);
cartesian_planner_->setMaxAccelerationScaling(1.0);
cartesian_planner_->setStepSize(.01);
addConnection(std::make_shared<MqttConnection>("localhost:1883", "asdfadf")); addConnection(std::make_shared<MqttConnection>("localhost:1883", "asdfadf"));
} }
...@@ -11,9 +69,375 @@ void MGMediator::mediate(){ ...@@ -11,9 +69,375 @@ void MGMediator::mediate(){
setPanel(); setPanel();
publishTables(); publishTables();
moveit::task_constructor::Task mgt1 = pickUp("A", "panda_arm1");
if(mgt1.plan(1)) {
//mgt.introspection().publishSolution(*mgt.solutions().front());
mgt1.execute(*mgt1.solutions().front());
}
}; };
moveit::task_constructor::Task MGMediator::pickUp(const std::string& obj, const std::string& robotId){
AbstractRobotDecorator* mr = nullptr;
try{
mr= robots_.at(robotId).get();
} catch (std::out_of_range& oor){
ROS_INFO("Robot not found");
ros::shutdown();
}
moveit_msgs::CollisionObject source = psi_->getObjects().at(obj);
const std::string object = source.id;
moveit::task_constructor::Task task_;
tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z));
std::string support_surface1 = "nichts";
for (const auto& s_r : robots_){
std::string str;
std::bitset<3> panel_mask(7);
if(s_r.second->checkSingleObjectCollision(t, str, panel_mask)) support_surface1 = str;
}
ROS_INFO("ss1 %s", support_surface1.c_str());
std::string name = "Pick&Place";
task_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec())));
task_.loadRobotModel();
task_.setRobotModel(mr->mgi()->getRobotModel());
task_.setProperty("group", mr->name());
task_.setProperty("eef", mr->map()["eef_name"]);
task_.setProperty("hand", mr->map()["hand_group_name"]);
task_.setProperty("hand_grasping_frame", mr->map()["hand_frame"]);
task_.setProperty("ik_frame", mr->map()["hand_frame"]);
moveit::task_constructor::Stage* current_state_ptr = nullptr;
{
auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>("current state");
auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>("applicability test", std::move(current_state));
applicability_filter->setPredicate([object](const moveit::task_constructor::SolutionBase& s, std::string& comment) {
if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
comment = "object with id '" + object + "' is already attached and cannot be picked";
return false;
}
return true;
});
current_state_ptr = applicability_filter.get();
task_.add(std::move(applicability_filter));
}
{ // Open Hand
auto stage = std::make_unique< moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_);
stage->setGroup(mr->map()["eef_name"]);
stage->setGoal("open");
task_.add(std::move(stage));
}
{ // Move-to pre-grasp
auto stage = std::make_unique< moveit::task_constructor::stages::Connect>(
"move to pick", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(), sampling_planner_} });
stage->setTimeout(5.0);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
task_.add(std::move(stage));
}
moveit::task_constructor::Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator
{
auto grasp = std::make_unique<moveit::task_constructor::SerialContainer>("pick object");
task_.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
grasp->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
{ // Approach Obj
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("approach object", cartesian_planner_);
stage->properties().set("marker_ns", "approach_object");
stage->properties().set("link", mr->map()["hand_frame"]);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(0.07, 0.2);
// Set hand forward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = mr->map()["hand_frame"];
vec.vector.z = 1.0;
stage->setDirection(vec);
grasp->insert(std::move(stage));
}
{
// Sample grasp pose
auto stage = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate grasp pose");
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
stage->properties().set("marker_ns", "grasp_pose");
stage->setPreGraspPose("open");
stage->setObject(object);
stage->setAngleDelta(M_PI / 2);
stage->setMonitoredStage(current_state_ptr); // Hook into current state
// Compute IK
Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
Eigen::Translation3d trans(0.1f, 0, 0);
Eigen::Isometry3d ik = eigen * trans;
auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("grasp pose IK", std::move(stage));
wrapper->setMaxIKSolutions(8);
wrapper->setMinSolutionDistance(1.0);
wrapper->setIKFrame(ik, mr->map()["hand_frame"]);
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
grasp->insert(std::move(wrapper));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
stage->allowCollisions(
object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
true);
grasp->insert(std::move(stage));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_);
stage->setGroup(mr->map()["eef_name"]);
stage->setGoal("close");
grasp->insert(std::move(stage));
}
{ // Attach obj
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("attach object");
stage->attachObject(object, mr->map()["hand_frame"]);
attach_object_stage = stage.get();
grasp->insert(std::move(stage));
}
{ // Allow Collision obj table
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (object,support)");
stage->allowCollisions({ object }, support_surface1, true);
grasp->insert(std::move(stage));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lift object", cartesian_planner_);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(0.1, 0.2);
stage->setIKFrame(mr->map()["hand_frame"]);
stage->properties().set("marker_ns", "lift_object");
// Set upward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = "world";
vec.vector.z = 1.0;
stage->setDirection(vec);
grasp->insert(std::move(stage));
}
{ // forbid collision
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)");
stage->allowCollisions({ object }, support_surface1, false);
grasp->insert(std::move(stage));
}
// Add grasp container to task
task_.add(std::move(grasp));
}
return task_;
}
moveit::task_constructor::Task MGMediator::place(const std::string& obj, const std::string& robotId, const std::string& target){
AbstractRobotDecorator* mr = nullptr;
try{
mr= robots_.at(robotId).get();
} catch (std::out_of_range& oor){
ROS_INFO("Robot not found");
ros::shutdown();
}
const std::string object = obj;
moveit::task_constructor::Task task_;
// Just some value
tf2::Transform t(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(-0.3f, -0.6f, 0.9355f));
std::string support_surface2;
for (const auto& s_r : robots_){
std::string str;
std::bitset<3> panel_mask(7);
if(s_r.second->checkSingleObjectCollision(t, str, panel_mask)) support_surface2 = str;
}
ROS_INFO("ss1 %s", support_surface2.c_str());
std::string name = "Pick&Place";
task_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec())));
task_.loadRobotModel();
task_.setRobotModel(mr->mgi()->getRobotModel());
task_.setProperty("group", mr->name());
task_.setProperty("eef", mr->map()["eef_name"]);
task_.setProperty("hand", mr->map()["hand_group_name"]);
task_.setProperty("hand_grasping_frame", mr->map()["hand_frame"]);
task_.setProperty("ik_frame", mr->map()["hand_frame"]);
moveit::task_constructor::Stage* current_state_ptr = nullptr;
{
auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>("current state");
auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>("applicability test", std::move(current_state));
applicability_filter->setPredicate([object](const moveit::task_constructor::SolutionBase& s, std::string& comment) {
if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
comment = "object with id '" + object + "' is already attached and cannot be picked";
// psst, I changed the return values and it works... dont tell this mama
return true;
}
return false;
});
current_state_ptr = applicability_filter.get();
task_.add(std::move(applicability_filter));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::Connect>(
"move to place", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(), sampling_planner_ } });
stage->setTimeout(5.0);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
task_.add(std::move(stage));
}
{
auto place = std::make_unique<moveit::task_constructor::SerialContainer>("place object");
task_.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
place->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group" });
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow cokbkmomsurface)");
stage->allowCollisions( {object} , support_surface2, true);
place->insert(std::move(stage));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lower object", cartesian_planner_);
stage->properties().set("marker_ns", "lower_object");
stage->properties().set("link", mr->map()["hand_frame"]);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(.03, .13);
// Set downward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = "world";
vec.vector.z = -1.0;
stage->setDirection(vec);
place->insert(std::move(stage));
}
{
// Generate Place Pose
auto stage = std::make_unique<moveit::task_constructor::stages::GeneratePlacePose>("generate place pose");
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "ik_frame" });
stage->properties().set("marker_ns", "place_pose");
stage->setObject(object);
// Set target pose
geometry_msgs::PoseStamped p;
p.header.frame_id = "world";
p.pose.position.x = t.getOrigin().getX();
p.pose.position.y = t.getOrigin().getY();
p.pose.position.z = t.getOrigin().getZ();
p.pose.orientation.x = t.getRotation().getX();
p.pose.orientation.y = t.getRotation().getY();
p.pose.orientation.z = t.getRotation().getZ();
p.pose.orientation.w = t.getRotation().getW();
stage->setPose(p);
stage->setMonitoredStage(current_state_ptr); // Hook into attach_object_stage
// Compute IK
Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
Eigen::Translation3d trans(0.1f, 0, 0);
Eigen::Isometry3d ik = eigen * trans;
auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("place pose IK", std::move(stage));
wrapper->setMaxIKSolutions(2);
wrapper->setIKFrame(ik, mr->map()["hand_frame"]);
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
place->insert(std::move(wrapper));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_);
stage->setGroup(mr->map()["eef_name"]);
stage->setGoal("open");
place->insert(std::move(stage));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
stage->allowCollisions(
object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
false);
place->insert(std::move(stage));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("detach object");
stage->detachObject(object, mr->map()["hand_frame"]);
place->insert(std::move(stage));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("retreat after place", cartesian_planner_);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(.1, .2);
stage->setIKFrame(mr->map()["hand_frame"]);
stage->properties().set("marker_ns", "retreat");
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = mr->map()["hand_frame"];
vec.vector.z = -1.0;
stage->setDirection(vec);
place->insert(std::move(stage));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_);
stage->setGroup(mr->map()["eef_name"]);
stage->setGoal("close");
place->insert(std::move(stage));
}
// Add place container to task
task_.add(std::move(place));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("move home", sampling_planner_);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setGoal("ready");
stage->restrictDirection(moveit::task_constructor::stages::MoveTo::FORWARD);
task_.add(std::move(stage));
}
return task_;
}
//! connect robot and initialize Moveit components //! connect robot and initialize Moveit components
/*! /*!
Set up acm_ and rs_ members to track, merge, and publish changes during execution. Set up acm_ and rs_ members to track, merge, and publish changes during execution.
...@@ -37,27 +461,60 @@ void MGMediator::connectRobots(std::unique_ptr<AbstractRobotDecorator> robot){ ...@@ -37,27 +461,60 @@ void MGMediator::connectRobots(std::unique_ptr<AbstractRobotDecorator> robot){
}; };
void MGMediator::publishTables(){ void MGMediator::publishTables(){
ros::WallDuration sleep_time(1.0); ros::WallDuration sleep_time(5.0);
// for (const auto& s_r : robots_){
// auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next()); std::string workSpace = "{\"mode\" : \"CONSTRUCT\", \"data\": {\"panda_arm1panda_arm2\" : [\"panda_arm1\", \"panda_arm2\"]}}";
sendToAll("init", workSpace);
// for (std::size_t k = 0; k < ceti_bot->observerMask().size(); k++){
// if(ceti_bot->observerMask()[k]){ for (const auto& s_r : robots_){
// auto wmd = dynamic_cast<MoveitPanel*>(ceti_bot->observers()[k]->next()); auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next());
// psi_->applyCollisionObject(wmd->marker());
// acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->marker().id.c_str(), std::vector<uint8_t>())); for (std::size_t k = 0; k < ceti_bot->observerMask().size(); k++){
// sleep_time.sleep(); if(ceti_bot->observerMask()[k]){
// } auto wmd = dynamic_cast<MoveitPanel*>(ceti_bot->observers()[k]->next());
// }
// } geometry_msgs::Vector3 size;
size.x = wmd->size().getX();
size.y = wmd->size().getY();
size.z = wmd->size().getZ();
geometry_msgs::Pose pose;
pose.position.x = wmd->marker().primitive_poses[0].position.x;
pose.position.y = wmd->marker().primitive_poses[0].position.y;
pose.position.z = wmd->marker().primitive_poses[0].position.z;
pose.orientation.x = wmd->marker().primitive_poses[0].orientation.x;
pose.orientation.y = wmd->marker().primitive_poses[0].orientation.y;
pose.orientation.z = wmd->marker().primitive_poses[0].orientation.z;
pose.orientation.w = wmd->marker().primitive_poses[0].orientation.w;
ROS_INFO("%f %f %f", size.x, size.y, size.z);
ROS_INFO("%f %f %f", pose.position.x, pose.position.y, pose.position.z);
ROS_INFO("%f %f %f %f", pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
// visual_tools_->publishCollisionCuboid(pose, size.x, size.y, size.z, wmd->name(),rviz_visual_tools::colors::DARK_GREY);
// visual_tools_->setAlpha(1.0f);
// visual_tools_->trigger();
psi_->applyCollisionObject(wmd->marker());
acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->marker().id.c_str(), std::vector<uint8_t>()));
sleep_time.sleep();
}
}
}
sendToAll("initcon", "");
std::string str; std::string str;
std::map<const std::string, std::vector<Cuboid>> robot_to_cuboid; std::map<const std::string, std::vector<Cuboid>> robot_to_cuboid;
std::bitset<3> panel_mask(7); std::bitset<3> panel_mask(7);
for (const auto& pair : robots_) { for (const auto& pair : robots_) {
robot_to_cuboid.insert_or_assign(pair.first, std::vector<Cuboid>()); robot_to_cuboid.insert_or_assign(pair.first, std::vector<Cuboid>());
std::stringstream ss; std::stringstream ss;
ss << "{\"objects\": [{\"id\": \""<< pair.first << "\", \"type\": \"ARM\"}"; ss << "{\"objects\": [{\"id\": \""<< pair.first << "\", \"type\": \"ARM\"}";
for (auto& c: cuboid_reader_->cuboidBox()){ for (auto& c: cuboid_reader_->cuboidBox()){
tf2::Transform trans(tf2::Quaternion(0,0,0,1), tf2::Vector3(c.Pose.position.x, c.Pose.position.y, c.Pose.position.z)); tf2::Transform trans(tf2::Quaternion(0,0,0,1), tf2::Vector3(c.Pose.position.x, c.Pose.position.y, c.Pose.position.z));
...@@ -65,6 +522,31 @@ void MGMediator::publishTables(){ ...@@ -65,6 +522,31 @@ void MGMediator::publishTables(){
ss << ",{\"id\": \"" << c.Name <<"\",\"type\": \"BOX\",\"pos\": {\"x\": "<< c.Pose.position.x << ",\"y\": "<< c.Pose.position.y <<",\"z\": " << c.Pose.position.x << "},\"size\": {\"length\": "<< c.x_depth <<",\"width\": "<< c.y_width <<",\"height\": "<< c.z_heigth <<"},\"orientation\": {\"x\": "<< c.Pose.orientation.x <<", \"y\": "<< c.Pose.orientation.y <<" , \"z\": "<< c.Pose.orientation.z <<", \"w\": "<< c.Pose.orientation.w<< "},"; ss << ",{\"id\": \"" << c.Name <<"\",\"type\": \"BOX\",\"pos\": {\"x\": "<< c.Pose.position.x << ",\"y\": "<< c.Pose.position.y <<",\"z\": " << c.Pose.position.x << "},\"size\": {\"length\": "<< c.x_depth <<",\"width\": "<< c.y_width <<",\"height\": "<< c.z_heigth <<"},\"orientation\": {\"x\": "<< c.Pose.orientation.x <<", \"y\": "<< c.Pose.orientation.y <<" , \"z\": "<< c.Pose.orientation.z <<", \"w\": "<< c.Pose.orientation.w<< "},";
ss << "\"color\": {\"r\": 0.2,\"g\": 0.2,\"b\": 0.2}}"; ss << "\"color\": {\"r\": 0.2,\"g\": 0.2,\"b\": 0.2}}";
} }
moveit_msgs::CollisionObject item;
item.id = c.Name;
item.header.frame_id = "world";
item.header.stamp = ros::Time::now();
item.primitives.resize(1);
item.primitives[0].type = item.primitives[0].BOX;
item.primitives[0].dimensions.resize(3);
item.primitives[0].dimensions[0] = c.x_depth;
item.primitives[0].dimensions[1] = c.y_width;
item.primitives[0].dimensions[2] = c.z_heigth;
item.primitive_poses.resize(1);
item.primitive_poses[0].position.x = c.Pose.position.x;
item.primitive_poses[0].position.y = c.Pose.position.y;
item.primitive_poses[0].position.z = c.Pose.position.z;
item.primitive_poses[0].orientation.x = c.Pose.orientation.x;
item.primitive_poses[0].orientation.y = c.Pose.orientation.y;
item.primitive_poses[0].orientation.z = c.Pose.orientation.z;
item.primitive_poses[0].orientation.w = c.Pose.orientation.w;
item.operation = item.ADD;
psi_->applyCollisionObject(item);
// acm_.insert(std::pair<std::string, std::vector<uint8_t>>(item.id, std::vector<uint8_t>()));
} }
for (auto& c: cuboid_reader_->cuboidObstacle()){ for (auto& c: cuboid_reader_->cuboidObstacle()){
...@@ -76,10 +558,15 @@ void MGMediator::publishTables(){ ...@@ -76,10 +558,15 @@ void MGMediator::publishTables(){
} }
ss << "]}"; ss << "]}";
std::stringstream rss;
rss << "World/panda_arm1panda_arm2/" << pair.first;
// publish // publish
sendToAll("World/robotarm1robotarm2", ss.str()); sleep_time.sleep();
sendToAll(rss.str(), ss.str());
} }
sendToAll("command/pickup", "");
} }
......
src/reader/cuboid_reader.cpp
+ 15
− 0
View file @ 2b1e4ab0
...@@ -26,6 +26,8 @@ void CuboidReader::read(){ ...@@ -26,6 +26,8 @@ void CuboidReader::read(){
(resources[i]["orientation"].hasMember("w")) ? rot.setW(floatOf(resources[i]["orientation"]["w"])) :rot.setW(0); (resources[i]["orientation"].hasMember("w")) ? rot.setW(floatOf(resources[i]["orientation"]["w"])) :rot.setW(0);
Cuboid o; Cuboid o;
o.Name = id.c_str(); o.Name = id.c_str();
o.Pose.position.x = pos.getX(); o.Pose.position.x = pos.getX();
o.Pose.position.y = pos.getY(); o.Pose.position.y = pos.getY();
...@@ -38,12 +40,19 @@ void CuboidReader::read(){ ...@@ -38,12 +40,19 @@ void CuboidReader::read(){
o.y_width = size.getY(); o.y_width = size.getY();
o.z_heigth = size.getZ(); o.z_heigth = size.getZ();
o.r = (resources[i]["color"].hasMember("r")) ? floatOf(resources[i]["color"]["r"]) : 0;
o.g = (resources[i]["color"].hasMember("g")) ? floatOf(resources[i]["color"]["g"]) : 0;
o.b = (resources[i]["color"].hasMember("b")) ? floatOf(resources[i]["color"]["b"]) : 0;
cuboid_box_.push_back(o); cuboid_box_.push_back(o);
ROS_INFO("--- Object: %s --- Type: BOX ---", o.Name.c_str()); ROS_INFO("--- Object: %s --- Type: BOX ---", o.Name.c_str());
ROS_INFO("=> Object: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ()); ROS_INFO("=> Object: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
ROS_INFO("=> Object: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW()); ROS_INFO("=> Object: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
ROS_INFO("=> Object: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ()); ROS_INFO("=> Object: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
ROS_INFO("=> Object: color('%f', '%f', '%f')", o.r, o.g, o.b);
} }
if(resources[i]["type"] == "BIN" || std::regex_match(id, match, std::regex("obstacle([0-9]+)"))){ if(resources[i]["type"] == "BIN" || std::regex_match(id, match, std::regex("obstacle([0-9]+)"))){
...@@ -76,11 +85,17 @@ void CuboidReader::read(){ ...@@ -76,11 +85,17 @@ void CuboidReader::read(){
o.y_width = size.getY(); o.y_width = size.getY();
o.z_heigth = size.getZ(); o.z_heigth = size.getZ();
o.r = (resources[i]["color"].hasMember("r")) ? floatOf(resources[i]["color"]["r"]) : 0;
o.g = (resources[i]["color"].hasMember("g")) ? floatOf(resources[i]["color"]["g"]) : 0;
o.b = (resources[i]["color"].hasMember("b")) ? floatOf(resources[i]["color"]["b"]) : 0;
cuboid_obstacle_.push_back(o); cuboid_obstacle_.push_back(o);
ROS_INFO("--- Object: %s --- Type: BIN ---", o.Name.c_str()); ROS_INFO("--- Object: %s --- Type: BIN ---", o.Name.c_str());
ROS_INFO("=> Object: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ()); ROS_INFO("=> Object: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
ROS_INFO("=> Object: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW()); ROS_INFO("=> Object: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
ROS_INFO("=> Object: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ()); ROS_INFO("=> Object: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
ROS_INFO("=> Object: color('%f', '%f', '%f')", o.r, o.g, o.b);
} }
} }
......
src/robot/ceti_robot.cpp
+ 1
− 1
View file @ 2b1e4ab0
...@@ -23,7 +23,7 @@ bool CetiRobot::checkSingleObjectCollision(tf2::Transform& obj, std::string& str ...@@ -23,7 +23,7 @@ bool CetiRobot::checkSingleObjectCollision(tf2::Transform& obj, std::string& str
std::stringstream ss; std::stringstream ss;
std::regex rx("panda_arm([0-9]+)"); std::regex rx("panda_arm([0-9]+)");
std::smatch match; std::smatch match;
std::regex_match(str, match, rx); std::regex_match(name(), match, rx);
ss << "base_" << match[1]; ss << "base_" << match[1];
A = tf_ * bounds_[0]; A = tf_ * bounds_[0];
......
src/robot/decorators/panda_decorator.cpp
+ 2
− 0
View file @ 2b1e4ab0
...@@ -20,6 +20,7 @@ void PandaDecorator::spezifieRootBounds(){ ...@@ -20,6 +20,7 @@ void PandaDecorator::spezifieRootBounds(){
void PandaDecorator::spezifieRobotGroups(){ void PandaDecorator::spezifieRobotGroups(){
mgi_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(next_->name()); mgi_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(next_->name());
std::stringstream hand_n, ik_frame_n, name_n, base_n; std::stringstream hand_n, ik_frame_n, name_n, base_n;
std::regex panda_id("panda_arm([0-9]+)"), left_finger("left_finger"), right_finger("right_finger"), hand_link("hand_link"); std::regex panda_id("panda_arm([0-9]+)"), left_finger("left_finger"), right_finger("right_finger"), hand_link("hand_link");
...@@ -30,6 +31,7 @@ void PandaDecorator::spezifieRobotGroups(){ ...@@ -30,6 +31,7 @@ void PandaDecorator::spezifieRobotGroups(){
ik_frame_n << "panda_" << match[1] << "_link8"; ik_frame_n << "panda_" << match[1] << "_link8";
base_n << "base_" << match[1]; base_n << "base_" << match[1];
mgi_hand_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(hand_n.str()); mgi_hand_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(hand_n.str());
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment