From 2b1e4ab06c8945546970ba86554c85a64300c7a4 Mon Sep 17 00:00:00 2001 From: KingMaZito <matteo.aneddama@icloud.com> Date: Mon, 17 Jul 2023 22:29:19 +0200 Subject: [PATCH] between MGs and Task Constructors --- include/mediator/mg_mediator.h | 31 ++ results/dummy/-2045918175/-2045918175.yaml | 6 +- src/mediator/mg_mediator.cpp | 521 ++++++++++++++++++++- src/reader/cuboid_reader.cpp | 15 + src/robot/ceti_robot.cpp | 2 +- src/robot/decorators/panda_decorator.cpp | 2 + 6 files changed, 557 insertions(+), 20 deletions(-) diff --git a/include/mediator/mg_mediator.h b/include/mediator/mg_mediator.h index 98eadbf..3c661f1 100644 --- a/include/mediator/mg_mediator.h +++ b/include/mediator/mg_mediator.h @@ -8,18 +8,47 @@ #include <ccf/connection/MqttConnection.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 class MGMediator: public AbstractMediator, virtual Controller{ protected: + std::shared_ptr<moveit::core::RobotModel> robot_model_; std::vector<std::unique_ptr<Connection>> connections_; 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<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::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::shared_ptr<planning_scene::PlanningScene> ps_; //!< Shared Planning Scene + std::string referenceRobot_; //!< Reference Robot @@ -46,6 +75,8 @@ class MGMediator: public AbstractMediator, virtual Controller{ void setPanel() override; 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 \ No newline at end of file diff --git a/results/dummy/-2045918175/-2045918175.yaml b/results/dummy/-2045918175/-2045918175.yaml index c82b215..98ff98e 100644 --- a/results/dummy/-2045918175/-2045918175.yaml +++ b/results/dummy/-2045918175/-2045918175.yaml @@ -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_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': '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': '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': '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': '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': '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 diff --git a/src/mediator/mg_mediator.cpp b/src/mediator/mg_mediator.cpp index 7f70d13..7bd9d80 100644 --- a/src/mediator/mg_mediator.cpp +++ b/src/mediator/mg_mediator.cpp @@ -3,7 +3,65 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh) : AbstractMediator(nh) , 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")); } @@ -11,9 +69,375 @@ void MGMediator::mediate(){ setPanel(); 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 /*! 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){ }; void MGMediator::publishTables(){ - ros::WallDuration sleep_time(1.0); - // for (const auto& s_r : robots_){ - // auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next()); - - // for (std::size_t k = 0; k < ceti_bot->observerMask().size(); k++){ - // if(ceti_bot->observerMask()[k]){ - // auto wmd = dynamic_cast<MoveitPanel*>(ceti_bot->observers()[k]->next()); - // 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(); - // } - // } - // } + ros::WallDuration sleep_time(5.0); + + std::string workSpace = "{\"mode\" : \"CONSTRUCT\", \"data\": {\"panda_arm1panda_arm2\" : [\"panda_arm1\", \"panda_arm2\"]}}"; + sendToAll("init", workSpace); + + for (const auto& s_r : robots_){ + auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next()); + + for (std::size_t k = 0; k < ceti_bot->observerMask().size(); k++){ + 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::map<const std::string, std::vector<Cuboid>> robot_to_cuboid; std::bitset<3> panel_mask(7); + for (const auto& pair : robots_) { robot_to_cuboid.insert_or_assign(pair.first, std::vector<Cuboid>()); - std::stringstream ss; + std::stringstream ss; + + ss << "{\"objects\": [{\"id\": \""<< pair.first << "\", \"type\": \"ARM\"}"; 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)); @@ -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 << "\"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()){ @@ -76,10 +558,15 @@ void MGMediator::publishTables(){ } ss << "]}"; + std::stringstream rss; + rss << "World/panda_arm1panda_arm2/" << pair.first; + // publish - sendToAll("World/robotarm1robotarm2", ss.str()); - } + sleep_time.sleep(); + sendToAll(rss.str(), ss.str()); + } + sendToAll("command/pickup", ""); } diff --git a/src/reader/cuboid_reader.cpp b/src/reader/cuboid_reader.cpp index 001da07..31609ca 100644 --- a/src/reader/cuboid_reader.cpp +++ b/src/reader/cuboid_reader.cpp @@ -26,6 +26,8 @@ void CuboidReader::read(){ (resources[i]["orientation"].hasMember("w")) ? rot.setW(floatOf(resources[i]["orientation"]["w"])) :rot.setW(0); Cuboid o; + + o.Name = id.c_str(); o.Pose.position.x = pos.getX(); o.Pose.position.y = pos.getY(); @@ -38,12 +40,19 @@ void CuboidReader::read(){ o.y_width = size.getY(); 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); 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: 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: 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]+)"))){ @@ -76,11 +85,17 @@ void CuboidReader::read(){ o.y_width = size.getY(); 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); 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: 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: color('%f', '%f', '%f')", o.r, o.g, o.b); + } } diff --git a/src/robot/ceti_robot.cpp b/src/robot/ceti_robot.cpp index 84bea3c..2da400a 100644 --- a/src/robot/ceti_robot.cpp +++ b/src/robot/ceti_robot.cpp @@ -23,7 +23,7 @@ bool CetiRobot::checkSingleObjectCollision(tf2::Transform& obj, std::string& str std::stringstream ss; std::regex rx("panda_arm([0-9]+)"); std::smatch match; - std::regex_match(str, match, rx); + std::regex_match(name(), match, rx); ss << "base_" << match[1]; A = tf_ * bounds_[0]; diff --git a/src/robot/decorators/panda_decorator.cpp b/src/robot/decorators/panda_decorator.cpp index 60a16bb..5767234 100644 --- a/src/robot/decorators/panda_decorator.cpp +++ b/src/robot/decorators/panda_decorator.cpp @@ -20,6 +20,7 @@ void PandaDecorator::spezifieRootBounds(){ void PandaDecorator::spezifieRobotGroups(){ mgi_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(next_->name()); + 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"); @@ -30,6 +31,7 @@ void PandaDecorator::spezifieRobotGroups(){ ik_frame_n << "panda_" << match[1] << "_link8"; base_n << "base_" << match[1]; + mgi_hand_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(hand_n.str()); -- GitLab