5 #include <gb_grasp/MapGenerator.h>
8 thread_local moveit::task_constructor::Task
task__;
9 thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal
etsg_;
16 ROS_INFO(
"%s connected...", robot->
name().c_str());
19 acm_.insert(std::pair<std::string, std::vector<uint8_t>>(mr->
map().at(
"base").c_str(), std::vector<uint8_t>()));
20 for(
auto name: mr->
mgi()->getLinkNames())
acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
21 for(
auto name: mr->
mgi_hand()->getLinkNames())
acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
25 ros::WallDuration sleep_time(1.0);
26 for(
long unsigned int i = 0; i <
robots_.size();i++){
55 acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->
markers()->id.c_str(), std::vector<uint8_t>()));
62 auto env =
ps_->getCollisionEnvNonConst();
71 ros::waitForShutdown();
77 std::bitset<3> result =
robots_[robot]->observer_mask() & wing;
80 for (std::size_t i = 0; i < result.size(); i++){
88 for (
int i =0; i < wbp.size(); i++){
89 std::vector<Abstract_robot_element*> v;
90 for (
int j =0; j < wbp[i].first.size(); j++){
99 ROS_INFO(
"=> write Task Constructor Objects");
100 for(
auto& a:
acm_) a.second.resize(
acm_.size(), 0);
545 std::regex item(
"box_([0-9]+)");
547 ROS_INFO(
"tasks %li",
tasks_.size());
550 ROS_INFO(
"in tasks");
551 std::vector<std::pair<std::string, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>> q_ets;
553 ROS_INFO(
"robot iteration");
554 auto itlow =
tasks_.lower_bound (r->name());
555 auto itup =
tasks_.upper_bound (r->name());
556 for (
auto it=itlow; it!=itup; ++it){
557 tf2::Vector3 b_start_position = (*it).second.first;
558 for(
auto& s_co :
psi_->getObjects()){
559 if(!std::regex_match(s_co.first, match, item))
continue;
560 if(tf2::tf2Distance2(b_start_position, tf2::Vector3(s_co.second.pose.position.x, s_co.second.pose.position.y,s_co.second.pose.position.z)) == 0) {
561 q_ets.push_back(std::pair<std::string, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>(r->name(), (*it).second.second));
569 ROS_INFO(
"tasks %li",
tasks_.size());
570 ROS_INFO(
"jobs %li", q_ets.size());
573 std::vector<std::thread> th;
575 while(!q_ets.empty()){
576 moveit_msgs::PlanningScene ps_m;
579 for (
auto& s_qets : q_ets){
580 if(s_qets.first == r->name()){
581 if(!s_qets.second.front().solution.sub_trajectory.empty()){
582 moveit_msgs::RobotTrajectory* rt = (!s_qets.second.front().solution.sub_trajectory.empty()) ? &s_qets.second.front().solution.sub_trajectory.front().trajectory :
nullptr;
583 moveit_msgs::PlanningScene* ps = (!s_qets.second.front().solution.sub_trajectory.empty()) ? &s_qets.second.front().solution.sub_trajectory.front().scene_diff :
nullptr;
591 std::regex rx_panda(
"panda_arm([0-9]+)");
593 std::regex_match(mr->
name(), match, rx_panda);
596 std::stringstream ss;
597 ss <<
"panda_" << match[1] <<
"_.*";
598 std::regex rx_panda_links(ss.str());
599 std::regex rx_box(
"box.*");
600 std::regex rx_table(
"table.*");
603 for(
int j = 0; j < ps->allowed_collision_matrix.entry_names.size(); j++ ){
604 if( mr->
map().at(
"base") == ps->allowed_collision_matrix.entry_names[j]){
605 for(
int k = 0; k < ps->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
606 int distance = std::distance(
acm_.begin(),
acm_.find(ps->allowed_collision_matrix.entry_names[k]));
607 if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
608 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
610 if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_box)){
611 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
613 if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_table)){
614 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
616 if (mr->
map().at(
"base")== ps->allowed_collision_matrix.entry_names[k]){
617 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
622 if(std::regex_match(ps->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
623 for(
int k = 0; k < ps->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
624 int distance = std::distance(
acm_.begin(),
acm_.find(ps->allowed_collision_matrix.entry_names[k]));
625 if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
626 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
628 if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_box)){
629 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
631 if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_table)){
632 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
634 if (mr->
map().at(
"base")== ps->allowed_collision_matrix.entry_names[k]){
635 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
640 if(std::regex_match(ps->allowed_collision_matrix.entry_names[j], match, rx_box)){
641 for(
int k = 0; k < ps->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
642 int distance = std::distance(
acm_.begin(),
acm_.find(ps->allowed_collision_matrix.entry_names[k]));
643 if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
644 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
646 if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_box)){
647 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
649 if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_table)){
650 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
652 if (mr->
map().at(
"base")== ps->allowed_collision_matrix.entry_names[k]){
653 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
658 if(std::regex_match(ps->allowed_collision_matrix.entry_names[j], match, rx_table)){
659 for(
int k = 0; k < ps->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
660 int distance = std::distance(
acm_.begin(),
acm_.find(ps->allowed_collision_matrix.entry_names[k]));
661 if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
662 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
664 if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_box)){
665 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
667 if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_table)){
668 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
670 if (mr->
map().at(
"base")== ps->allowed_collision_matrix.entry_names[k]){
671 acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
678 s_qets.second.front().solution.sub_trajectory.erase(s_qets.second.front().solution.sub_trajectory.begin());
680 }
else {s_qets.second.pop();};
684 for(
int i = 0; i < th.size(); i++){
685 if(th[i].joinable()) th[i].join();
706 for (
int i = 0; i < cd.size(); i ++){
707 std::stringstream ss;
710 moveit_msgs::CollisionObject item;
712 item.header.frame_id =
"world";
713 item.header.stamp = ros::Time::now();
714 item.primitives.resize(1);
715 item.primitives[0].type = item.primitives[0].BOX;
716 item.primitives[0].dimensions.resize(3);
717 item.primitives[0].dimensions[0] = cd[i].x_depth;
718 item.primitives[0].dimensions[1] = cd[i].y_width;
719 item.primitives[0].dimensions[2] = cd[i].z_heigth;
721 item.primitive_poses.resize(1);
722 item.primitive_poses[0].position.x = cd[i].Pose.position.x;
723 item.primitive_poses[0].position.y = cd[i].Pose.position.y;
724 item.primitive_poses[0].position.z = cd[i].Pose.position.z;
725 item.primitive_poses[0].orientation.x = cd[i].Pose.orientation.x;
726 item.primitive_poses[0].orientation.y = cd[i].Pose.orientation.y;
727 item.primitive_poses[0].orientation.z = cd[i].Pose.orientation.z;
728 item.primitive_poses[0].orientation.w = cd[i].Pose.orientation.w;
729 item.operation = item.ADD;
731 psi_->applyCollisionObject(item);
732 acm_.insert(std::pair<std::string, std::vector<uint8_t>>(item.id, std::vector<uint8_t>()));
737 std::regex item(
"box_([0-9]+)");
742 for(
auto& s_co :
psi_->getObjects()){
743 if(!std::regex_match(s_co.first, match, item))
continue;
744 std::pair<std::string, job_data> temp = jq.front();
745 ROS_INFO(
"1. job entry %f %f %f", temp.second.jobs_.front().getOrigin().getX(), temp.second.jobs_.front().getOrigin().getY(), temp.second.jobs_.front().getOrigin().getZ());
746 ROS_INFO(
"object position %f %f %f", s_co.second.pose.position.x, s_co.second.pose.position.y, s_co.second.pose.position.z);
747 if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), tf2::Vector3(s_co.second.pose.position.x, s_co.second.pose.position.y, s_co.second.pose.position.z )) == 0) {
748 std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> tasks_per_robot;
754 for (
int k = 1; k < temp.second.jobs_.size(); k++){
755 moveit::task_constructor::Task mgt =
create_Task(mr,
psi_->getObjects().at(s_co.first), temp.second.jobs_[k]);
757 moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
758 mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
759 tasks_per_robot.push(e);
761 moveit_msgs::CollisionObject temp_co =
psi_->getObjects().at(s_co.first);
762 temp_co.operation = temp_co.MOVE;
763 temp_co.pose.position.x = temp.second.jobs_[k].getOrigin().getX();
764 temp_co.pose.position.y = temp.second.jobs_[k].getOrigin().getY();
765 temp_co.pose.position.z = temp.second.jobs_[k].getOrigin().getZ();
766 temp_co.pose.orientation.x = temp.second.jobs_[k].getRotation().getX();
767 temp_co.pose.orientation.y = temp.second.jobs_[k].getRotation().getY();
768 temp_co.pose.orientation.z = temp.second.jobs_[k].getRotation().getZ();
769 temp_co.pose.orientation.w = temp.second.jobs_[k].getRotation().getW();
770 psi_->applyCollisionObject(temp_co);
773 std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>> pair(temp.second.jobs_.front().getOrigin(), tasks_per_robot);
774 tasks_.insert(std::pair<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>(mr->
name(), pair));
776 }
else {jq.push_back(temp);}
781 for (
int i = 0; i < cd.size(); i ++){
782 std::stringstream ss;
785 moveit_msgs::CollisionObject item;
787 item.header.frame_id =
"world";
788 item.header.stamp = ros::Time::now();
789 item.primitives.resize(1);
790 item.primitives[0].type = item.primitives[0].BOX;
791 item.primitives[0].dimensions.resize(3);
792 item.primitives[0].dimensions[0] = cd[i].x_depth;
793 item.primitives[0].dimensions[1] = cd[i].y_width;
794 item.primitives[0].dimensions[2] = cd[i].z_heigth;
796 item.pose.position.x = cd[i].Pose.position.x;
797 item.pose.position.y = cd[i].Pose.position.y;
798 item.pose.position.z = cd[i].Pose.position.z;
799 item.pose.orientation.x = cd[i].Pose.orientation.x;
800 item.pose.orientation.y = cd[i].Pose.orientation.y;
801 item.pose.orientation.z = cd[i].Pose.orientation.z;
802 item.pose.orientation.w = cd[i].Pose.orientation.w;
803 item.operation = item.MOVE;
805 psi_->applyCollisionObject(item);
813 moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
814 acmt.entry_values.resize(
acm_.size());
817 for (
auto& a :
acm_){
818 acmt.entry_names.push_back(a.first);
819 acmt.entry_values[i].enabled = a.second;
823 ps_m.allowed_collision_matrix = acmt;
824 ROS_INFO(
"broken after merge");
830 std::vector<std::string> links;
831 for (
auto link : mr->
mgi()->getLinkNames())links.push_back(link);
832 links.push_back(mr->
map()[
"left_finger"]);
833 links.push_back(mr->
map()[
"right_finger"]);
834 links.push_back(mr->
map()[
"hand_link"]);
835 links.push_back(mr->
map()[
"base"]);
838 for (
auto ao : in->robot_state.attached_collision_objects) out.robot_state.attached_collision_objects.push_back(ao);
839 if (in->robot_state.is_diff) out.robot_state.is_diff = in->robot_state.is_diff;
840 if (in->is_diff) out.is_diff = in->is_diff;
841 out.robot_state.joint_state.header = in->robot_state.joint_state.header;
842 out.robot_model_name =
"panda";
846 for (
auto link : links) {
847 for(
int i = 0; i < in->robot_state.joint_state.name.size(); i++){
848 if(link.c_str() == in->robot_state.joint_state.name[i].c_str()){
849 out.robot_state.joint_state.effort.push_back(in->robot_state.joint_state.effort[i]);
850 out.robot_state.joint_state.position.push_back(in->robot_state.joint_state.position[i]);
851 out.robot_state.joint_state.velocity.push_back(in->robot_state.joint_state.velocity[i]);
856 for(
int i = 0; i < in->link_padding.size(); i++){
857 if(link.c_str() == in->link_padding[i].link_name.c_str()){
858 out.link_padding.push_back(in->link_padding[i]);
862 for(
int i = 0; i < in->link_scale.size(); i++){
863 if(link.c_str() == in->link_scale[i].link_name.c_str()){
864 out.link_scale.push_back(in->link_scale[i]);
872 mr.
mgi()->execute(rt);
878 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));
879 std::string support_surface1 =
"nichts";
880 std::string support_surface2 =
"nichts";
885 if(ar->check_single_object_collision(t, str)) support_surface1 = str;
886 if(ar->check_single_object_collision(target, str)) support_surface2= str;
889 ROS_INFO(
"ss1 %s", support_surface1.c_str());
890 ROS_INFO(
"ss2 %s", support_surface2.c_str());
893 const std::string
object = source.id;
894 moveit::task_constructor::Task task_;
896 std::string name =
"Pick&Place";
897 task_.stages()->setName(name + std::to_string(
static_cast<int>(ros::Time::now().toNSec())));
898 task_.loadRobotModel();
899 task_.setRobotModel(mr->
mgi()->getRobotModel());
902 task_.setProperty(
"group", mr->
name());
903 task_.setProperty(
"eef", mr->
map()[
"eef_name"]);
904 task_.setProperty(
"hand", mr->
map()[
"hand_group_name"]);
905 task_.setProperty(
"hand_grasping_frame", mr->
map()[
"hand_frame"]);
906 task_.setProperty(
"ik_frame", mr->
map()[
"hand_frame"]);
908 moveit::task_constructor::Stage* current_state_ptr =
nullptr;
910 auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>(
"current state");
911 auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>(
"applicability test", std::move(current_state));
912 applicability_filter->setPredicate([
object](
const moveit::task_constructor::SolutionBase& s, std::string& comment) {
913 if (s.start()->scene()->getCurrentState().hasAttachedBody(
object)) {
914 comment =
"object with id '" + object +
"' is already attached and cannot be picked";
920 current_state_ptr = applicability_filter.get();
921 task_.add(std::move(applicability_filter));
925 auto stage = std::make_unique< moveit::task_constructor::stages::MoveTo>(
"open hand",
sampling_planner_);
926 stage->setGroup(mr->
map()[
"eef_name"]);
927 stage->setGoal(
"open");
928 task_.add(std::move(stage));
932 auto stage = std::make_unique< moveit::task_constructor::stages::Connect>(
933 "move to pick", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->
name(),
sampling_planner_} });
934 stage->setTimeout(5.0);
935 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
936 task_.add(std::move(stage));
939 moveit::task_constructor::Stage* attach_object_stage =
nullptr;
941 auto grasp = std::make_unique<moveit::task_constructor::SerialContainer>(
"pick object");
942 task_.properties().exposeTo(grasp->properties(), {
"eef",
"hand",
"group",
"ik_frame" });
943 grasp->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"eef",
"hand",
"group",
"ik_frame" });
947 auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>(
"approach object",
cartesian_planner_);
948 stage->properties().set(
"marker_ns",
"approach_object");
949 stage->properties().set(
"link", mr->
map()[
"hand_frame"]);
950 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"group" });
951 stage->setMinMaxDistance(0.07, 0.2);
954 geometry_msgs::Vector3Stamped vec;
955 vec.header.frame_id = mr->
map()[
"hand_frame"];
957 stage->setDirection(vec);
958 grasp->insert(std::move(stage));
963 auto stage = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>(
"generate grasp pose");
964 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
965 stage->properties().set(
"marker_ns",
"grasp_pose");
966 stage->setPreGraspPose(
"open");
967 stage->setObject(
object);
968 stage->setAngleDelta(M_PI / 12);
969 stage->setMonitoredStage(current_state_ptr);
972 Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
973 Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
974 Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
975 Eigen::Translation3d trans(0.1f, 0, 0);
976 Eigen::Isometry3d ik = eigen * trans;
978 auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>(
"grasp pose IK", std::move(stage));
979 wrapper->setMaxIKSolutions(8);
980 wrapper->setMinSolutionDistance(1.0);
981 wrapper->setIKFrame(ik, mr->
map()[
"hand_frame"]);
982 wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"eef",
"group" });
983 wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, {
"target_pose" });
984 grasp->insert(std::move(wrapper));
988 auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>(
"allow collision (hand,object)");
989 stage->allowCollisions(
990 object, task_.getRobotModel()->getJointModelGroup(mr->
map()[
"eef_name"])->getLinkModelNamesWithCollisionGeometry(),
992 grasp->insert(std::move(stage));
996 auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>(
"close hand",
sampling_planner_);
997 stage->setGroup(mr->
map()[
"eef_name"]);
998 stage->setGoal(
"close");
999 grasp->insert(std::move(stage));
1003 auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>(
"attach object");
1004 stage->attachObject(
object, mr->
map()[
"hand_frame"]);
1005 attach_object_stage = stage.get();
1006 grasp->insert(std::move(stage));
1010 auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>(
"allow collision (object,support)");
1011 stage->allowCollisions({
object }, support_surface1,
true);
1012 grasp->insert(std::move(stage));
1016 auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>(
"lift object",
cartesian_planner_);
1017 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"group" });
1018 stage->setMinMaxDistance(0.1, 0.2);
1019 stage->setIKFrame(mr->
map()[
"hand_frame"]);
1020 stage->properties().set(
"marker_ns",
"lift_object");
1023 geometry_msgs::Vector3Stamped vec;
1024 vec.header.frame_id =
"world";
1026 stage->setDirection(vec);
1027 grasp->insert(std::move(stage));
1031 auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>(
"forbid collision (object,surface)");
1032 stage->allowCollisions({
object }, support_surface1,
false);
1033 grasp->insert(std::move(stage));
1037 task_.add(std::move(grasp));
1041 auto stage = std::make_unique<moveit::task_constructor::stages::Connect>(
1042 "move to place", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->
name(),
sampling_planner_ } });
1043 stage->setTimeout(5.0);
1044 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
1045 task_.add(std::move(stage));
1049 auto place = std::make_unique<moveit::task_constructor::SerialContainer>(
"place object");
1050 task_.properties().exposeTo(place->properties(), {
"eef",
"hand",
"group" });
1051 place->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"eef",
"hand",
"group" });
1054 auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>(
"allow cokbkmomsurface)");
1055 stage->allowCollisions( {
object} , support_surface2,
true);
1056 place->insert(std::move(stage));
1061 auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>(
"lower object",
cartesian_planner_);
1062 stage->properties().set(
"marker_ns",
"lower_object");
1063 stage->properties().set(
"link", mr->
map()[
"hand_frame"]);
1064 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"group" });
1065 stage->setMinMaxDistance(.03, .13);
1068 geometry_msgs::Vector3Stamped vec;
1069 vec.header.frame_id =
"world";
1070 vec.vector.z = -1.0;
1071 stage->setDirection(vec);
1072 place->insert(std::move(stage));
1077 auto stage = std::make_unique<moveit::task_constructor::stages::GeneratePlacePose>(
"generate place pose");
1078 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"ik_frame" });
1079 stage->properties().set(
"marker_ns",
"place_pose");
1080 stage->setObject(
object);
1083 geometry_msgs::PoseStamped p;
1084 p.header.frame_id =
"world";
1085 p.pose.position.x = target.getOrigin().getX();
1086 p.pose.position.y = target.getOrigin().getY();
1087 p.pose.position.z = target.getOrigin().getZ();
1088 p.pose.orientation.x = target.getRotation().getX();
1089 p.pose.orientation.y = target.getRotation().getY();
1090 p.pose.orientation.z = target.getRotation().getZ();
1091 p.pose.orientation.w = target.getRotation().getW();
1096 stage->setMonitoredStage(attach_object_stage);
1099 Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
1100 Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
1101 Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
1102 Eigen::Translation3d trans(0.1f, 0, 0);
1103 Eigen::Isometry3d ik = eigen * trans;
1104 auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>(
"place pose IK", std::move(stage));
1105 wrapper->setMaxIKSolutions(2);
1106 wrapper->setIKFrame(ik, mr->
map()[
"hand_frame"]);
1107 wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"eef",
"group" });
1108 wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, {
"target_pose" });
1109 place->insert(std::move(wrapper));
1113 auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>(
"open hand",
sampling_planner_);
1114 stage->setGroup(mr->
map()[
"eef_name"]);
1115 stage->setGoal(
"open");
1116 place->insert(std::move(stage));
1120 auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>(
"allow collision (hand,object)");
1121 stage->allowCollisions(
1122 object, task_.getRobotModel()->getJointModelGroup(mr->
map()[
"eef_name"])->getLinkModelNamesWithCollisionGeometry(),
1124 place->insert(std::move(stage));
1128 auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>(
"detach object");
1129 stage->detachObject(
object, mr->
map()[
"hand_frame"]);
1130 place->insert(std::move(stage));
1134 auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>(
"retreat after place",
cartesian_planner_);
1135 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"group" });
1136 stage->setMinMaxDistance(.1, .2);
1137 stage->setIKFrame(mr->
map()[
"hand_frame"]);
1138 stage->properties().set(
"marker_ns",
"retreat");
1139 geometry_msgs::Vector3Stamped vec;
1140 vec.header.frame_id = mr->
map()[
"hand_frame"];
1141 vec.vector.z = -1.0;
1142 stage->setDirection(vec);
1143 place->insert(std::move(stage));
1147 task_.add(std::move(place));
1151 auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>(
"move home",
sampling_planner_);
1152 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"group" });
1153 stage->setGoal(
"ready");
1154 stage->restrictDirection(moveit::task_constructor::stages::MoveTo::FORWARD);
1155 task_.add(std::move(stage));
1163 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));
1166 std::string support_surface1 =
"nichts";
1167 std::string support_surface2 =
"nichts";
1169 XmlRpc::XmlRpcValue task;
1170 nh_->getParam(
"task/stages", task);
1176 if(ar->check_single_object_collision(t, str)) support_surface1 = str;
1177 if(ar->check_single_object_collision(target, str)) support_surface2= str;
1182 ROS_INFO(
"%f %f %f source", t.getOrigin().getX(), t.getOrigin().getY(), t.getOrigin().getZ());
1184 ROS_INFO(
"surface1 %s", support_surface1.c_str());
1185 ROS_INFO(
"surface2 %s", support_surface2.c_str());
1188 nh_->setParam(
"/task/properties/group", r->
name());
1189 nh_->setParam(
"/task/properties/eef", mr->
map()[
"eef"]);
1190 nh_->setParam(
"/task/properties/hand_grasping_frame", mr->
map()[
"hand_grasping_frame"]);
1191 nh_->setParam(
"/task/properties/ik_frame", mr->
map()[
"ik_frame"]);
1192 nh_->setParam(
"/task/properties/hand", mr->
map()[
"hand"]);
1194 XmlRpc::XmlRpcValue a, b, c, d, e, h;
1195 a[
"group"] = mr->
map()[
"hand"];
1196 e[
"joint_model_group_name"] = mr->
map()[
"hand"];
1198 b = task[4][
"stages"];
1199 b[1][
"stage"][
"properties"][
"object"] = source.id;
1200 b[3][
"properties"] = a;
1201 b[2][
"set"][
"allow_collisions"][
"first"] = source.id;
1202 b[2][
"set"][
"allow_collisions"][
"second"] = e;
1204 c = task[6][
"stages"];
1206 c[0][
"set"][
"allow_collisions"][
"first"] = source.id;
1207 c[0][
"set"][
"allow_collisions"][
"second"] = support_surface2;
1208 c[3][
"properties"] = a;
1209 c[6][
"properties"] = a;
1210 c[4][
"set"][
"allow_collisions"][
"first"] = source.id;
1211 c[4][
"set"][
"allow_collisions"][
"second"] = e;
1213 c[2][
"stage"][
"set"][
"pose"][
"point"][
"x"]=
static_cast<double>(target.getOrigin().getX());
1214 c[2][
"stage"][
"set"][
"pose"][
"point"][
"y"]=
static_cast<double>(target.getOrigin().getY());
1215 c[2][
"stage"][
"set"][
"pose"][
"point"][
"z"]=
static_cast<double>(target.getOrigin().getZ());
1218 task[2][
"properties"] = a;
1220 XmlRpc::XmlRpcValue connect, f, g;
1221 f[r->
name()] =
"sampling";
1222 g[
"source"] =
"PARENT";
1223 connect[
"type"] =
"Connect";
1224 connect[
"group_planner_vector"] = f;
1225 connect[
"propertiesConfigureInitFrom"] = g;
1231 a[
"link"] = mr->
map()[
"ik_frame"];
1232 a[
"min_distance"] = 0.07;
1233 a[
"max_distance"] = 0.2;
1234 c[5][
"properties"] = a;
1235 b[0][
"properties"] = a;
1236 a[
"min_distance"] = 0.1;
1237 a[
"max_distance"] = 0.2;
1238 c[1][
"properties"] = a;
1240 e[
"object"] =
"bottle";
1241 e[
"link"] = mr->
map()[
"ik_frame"];
1242 b[4][
"set"][
"attach_object"] = e;
1243 c[4][
"set"][
"detach_object"] = e;
1244 c[2][
"set"][
"ik_frame"][
"link"] = mr->
map()[
"ik_frame"];
1245 c[5][
"set"][
"direction"][
"vector"][
"header"][
"frame_id"] = mr->
map()[
"ik_frame"];
1246 b[1][
"set"][
"ik_frame"][
"link"] = mr->
map()[
"ik_frame"];
1247 b[6][
"set"][
"ik_frame"][
"link"] = mr->
map()[
"ik_frame"];
1248 b[0][
"set"][
"direction"][
"vector"][
"header"][
"frame_id"] = mr->
map()[
"ik_frame"];
1250 b[5][
"set"][
"allow_collisions"][
"first"] = source.id;
1251 b[5][
"set"][
"allow_collisions"][
"second"] = support_surface1;
1252 b[7][
"set"][
"allow_collisions"][
"first"] = source.id;
1253 b[7][
"set"][
"allow_collisions"][
"second"] = support_surface1;
1255 task[6][
"stages"] = c;
1256 task[4][
"stages"] = b;
1258 nh_->setParam(
"task/stages", task);
1264 , sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
1265 , cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
1266 , psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
1267 , mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>(
"panda_arms"))
1268 , planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>(
"planning_scene", 1)))
1269 , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
"robot_description"))
1270 , job_reader_(std::make_unique<
Job_reader>(nh_))
1275 planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
1279 ROS_ERROR_STREAM_NAMED(
"test",
"Planning scene not configured");
1283 robot_model_loader::RobotModelLoaderPtr robot_model_loader;
1284 robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>(
"robot_description");