7 #include <gb_grasp/MapGenerator.h>
9 #include <actionlib_msgs/GoalStatusArray.h>
14 , sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
15 , cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
16 , psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
17 , mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>(
"panda_arms"))
18 , planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>(
"planning_scene", 1)))
19 , job_reader_(std::make_unique<
JobReader>(nh_)){
21 robot_model_loader::RobotModelLoaderPtr robot_model_loader;
22 robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>(
"robot_description");
37 ROS_INFO(
"connecting %s...", robot->name().c_str());
38 robot->spezifieRobotGroups();
41 acm_.insert(std::pair<std::string, std::vector<uint8_t>>(robot->map().at(
"base").c_str(), std::vector<uint8_t>()));
44 for(
auto name: robot->mgi()->getLinkNames())
acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
45 for(
auto name: robot->mgiHand()->getLinkNames())
acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
47 for (
auto link : robot->mgi()->getLinkNames())
rs_.insert_or_assign(link, std::vector<std::uint8_t>());
48 rs_.insert_or_assign(robot->map()[
"left_finger"], std::vector<std::uint8_t>());
49 rs_.insert_or_assign(robot->map()[
"right_finger"], std::vector<std::uint8_t>());
50 rs_.insert_or_assign(robot->map()[
"hand_link"], std::vector<std::uint8_t>());
51 rs_.insert_or_assign(robot->map()[
"base"], std::vector<std::uint8_t>());
53 robots_.insert_or_assign(robot->name(), std::move(robot));
60 for (
const auto& s_r :
robots_){
62 auto ceti_bot =
dynamic_cast<CetiRobot*
>(s_r.second->next());
64 }
catch(
const std::out_of_range& oor) {
65 ROS_INFO(
"No mask data for %s", s_r.first.c_str());
69 for (
const auto& s_r :
robots_){
70 auto ceti_bot =
dynamic_cast<CetiRobot*
>(s_r.second->next());
71 std::vector<std::unique_ptr<AbstractRobotElementDecorator>> panels(3);
73 for (std::size_t j = 0; j < ceti_bot->observerMask().size(); j++){
74 if (ceti_bot->observerMask()[j]){
76 tf2::Transform relative_tf = ceti_bot->tf().inverse() * wd.at(ceti_bot->name()).first[j].pose_;
78 std::unique_ptr<AbstractRobotElement> panel = std::make_unique<MoveitPanel>(wd.at(s_r.first).first[j].name_, relative_tf, wd.at(s_r.first).first[j].size_);
79 std::unique_ptr<AbstractRobotElementDecorator> log = std::make_unique<LogDecorator>(std::move(panel));
80 panels[j] = std::move(log);
81 }
catch (std::out_of_range &oor){
82 ROS_INFO(
"OOR in set_panel");
86 ceti_bot->setObservers(panels);
89 for (
const auto& s_r :
robots_){
95 ros::WallDuration sleep_time(1.0);
97 for (
const auto& s_r :
robots_){
98 auto ceti_bot =
dynamic_cast<CetiRobot*
>(s_r.second->next());
100 for (std::size_t k = 0; k < ceti_bot->observerMask().size(); k++){
101 if(ceti_bot->observerMask()[k]){
102 auto wmd =
dynamic_cast<MoveitPanel*
>(ceti_bot->observers()[k]->next());
103 psi_->applyCollisionObject(wmd->marker());
104 acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->marker().id.c_str(), std::vector<uint8_t>()));
115 ros::waitForShutdown();
120 std::regex rx_panda(mr->
pattern());
122 std::regex_match(mr->
name(), match, rx_panda);
125 std::stringstream ss;
126 ss <<
"panda_" << match[1] <<
"_.*";
127 std::regex rx_panda_links(ss.str());
128 std::regex rx_box(
"box_.*");
129 std::regex rx_table(
"table.*");
133 for(
int j = 0; j < ps.allowed_collision_matrix.entry_names.size(); j++ ){
134 if( mr->map().at(
"base") == ps.allowed_collision_matrix.entry_names[j]){
137 for(
int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
139 int distance = std::distance(
acm_.begin(),
acm_.find(ps.allowed_collision_matrix.entry_names[k]));
141 if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
142 acm_.at(mr->map().at(
"base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
145 if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
146 acm_.at(mr->map().at(
"base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
149 if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
150 acm_.at(mr->map().at(
"base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
153 if (mr->map().at(
"base")== ps.allowed_collision_matrix.entry_names[k]){
154 acm_.at(mr->map().at(
"base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
159 if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
161 ROS_INFO(
"entry matches link %s at index %i", ps.allowed_collision_matrix.entry_names[j].c_str(), j);
162 for(
int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
164 int distance = std::distance(
acm_.begin(),
acm_.find(ps.allowed_collision_matrix.entry_names[k]));
166 if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
167 acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
170 if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
171 acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
174 if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
175 acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
178 if (mr->map().at(
"base")== ps.allowed_collision_matrix.entry_names[k]){
179 acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
184 if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_box)){
186 for(
int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
188 int distance = std::distance(
acm_.begin(),
acm_.find(ps.allowed_collision_matrix.entry_names[k]));
190 if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
191 acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
194 if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
195 acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
198 if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
199 acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
202 if (mr->map().at(
"base")== ps.allowed_collision_matrix.entry_names[k]){
203 acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
208 if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_table)){
210 for(
int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
212 int distance = std::distance(
acm_.begin(),
acm_.find(ps.allowed_collision_matrix.entry_names[k]));
214 if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
215 acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
218 if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
219 acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
222 if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
223 acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
226 if (mr->map().at(
"base")== ps.allowed_collision_matrix.entry_names[k]){
227 acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
246 for (
int i = 0; i < cd.size(); i ++){
247 std::stringstream ss;
250 moveit_msgs::CollisionObject item;
252 item.header.frame_id =
"world";
253 item.header.stamp = ros::Time::now();
254 item.primitives.resize(1);
255 item.primitives[0].type = item.primitives[0].BOX;
256 item.primitives[0].dimensions.resize(3);
257 item.primitives[0].dimensions[0] = cd[i].x_depth;
258 item.primitives[0].dimensions[1] = cd[i].y_width;
259 item.primitives[0].dimensions[2] = cd[i].z_heigth;
261 item.primitive_poses.resize(1);
262 item.primitive_poses[0].position.x = cd[i].Pose.position.x;
263 item.primitive_poses[0].position.y = cd[i].Pose.position.y;
264 item.primitive_poses[0].position.z = cd[i].Pose.position.z;
265 item.primitive_poses[0].orientation.x = cd[i].Pose.orientation.x;
266 item.primitive_poses[0].orientation.y = cd[i].Pose.orientation.y;
267 item.primitive_poses[0].orientation.z = cd[i].Pose.orientation.z;
268 item.primitive_poses[0].orientation.w = cd[i].Pose.orientation.w;
269 item.operation = item.ADD;
271 psi_->applyCollisionObject(item);
272 acm_.insert(std::pair<std::string, std::vector<uint8_t>>(item.id, std::vector<uint8_t>()));
278 for(
auto& a:
acm_) a.second.resize(
acm_.size(), 0);
280 std::regex item(
"box_([0-9]+)");
287 BT::BehaviorTreeFactory factory;
288 factory.registerNodeType<
Execution>(
"Execution");
292 std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;
295 for(
auto& s_co :
psi_->getObjects()){
296 if(!std::regex_match(s_co.first, match, item))
continue;
298 std::pair<std::string, job_data> temp = jq.front();
299 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());
300 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);
301 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) {
302 std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> bt_list;
306 ard=
robots_.at(temp.first).get();
307 }
catch (std::out_of_range& oor){
308 ROS_INFO(
"Robot not found");
313 for (
int k = 1; k < temp.second.jobs_.size(); k++){
314 moveit::task_constructor::Task mgt =
createTask(ard,
psi_->getObjects().at(s_co.first), temp.second.jobs_[k]);
317 moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
318 mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
319 bt_list.push_back(e);
321 moveit_msgs::CollisionObject temp_co =
psi_->getObjects().at(s_co.first);
322 temp_co.operation = temp_co.MOVE;
323 temp_co.pose.position.x = temp.second.jobs_[k].getOrigin().getX();
324 temp_co.pose.position.y = temp.second.jobs_[k].getOrigin().getY();
325 temp_co.pose.position.z = temp.second.jobs_[k].getOrigin().getZ();
326 temp_co.pose.orientation.x = temp.second.jobs_[k].getRotation().getX();
327 temp_co.pose.orientation.y = temp.second.jobs_[k].getRotation().getY();
328 temp_co.pose.orientation.z = temp.second.jobs_[k].getRotation().getZ();
329 temp_co.pose.orientation.w = temp.second.jobs_[k].getRotation().getW();
330 psi_->applyCollisionObject(temp_co);
341 for (
int i = 0; i < cd.size(); i ++){
342 std::stringstream ss;
345 moveit_msgs::CollisionObject item;
347 item.header.frame_id =
"world";
348 item.header.stamp = ros::Time::now();
349 item.primitives.resize(1);
350 item.primitives[0].type = item.primitives[0].BOX;
351 item.primitives[0].dimensions.resize(3);
352 item.primitives[0].dimensions[0] = cd[i].x_depth;
353 item.primitives[0].dimensions[1] = cd[i].y_width;
354 item.primitives[0].dimensions[2] = cd[i].z_heigth;
356 item.pose.position.x = cd[i].Pose.position.x;
357 item.pose.position.y = cd[i].Pose.position.y;
358 item.pose.position.z = cd[i].Pose.position.z;
359 item.pose.orientation.x = cd[i].Pose.orientation.x;
360 item.pose.orientation.y = cd[i].Pose.orientation.y;
361 item.pose.orientation.z = cd[i].Pose.orientation.z;
362 item.pose.orientation.w = cd[i].Pose.orientation.w;
363 item.operation = item.MOVE;
365 psi_->applyCollisionObject(item);
371 task_but_different.at(ard->
name()).push_back(std::tuple<std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>(s_co.first, temp.second.jobs_.front().getOrigin(), bt_list));
372 }
catch(std::out_of_range &oor) {
373 std::tuple<const std::string&, tf2::Vector3&, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>&> tuple(s_co.first, temp.second.jobs_.front().getOrigin(), bt_list);
374 task_but_different.insert(std::pair<std::string, std::vector<std::tuple<
const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>>(ard->
name(),{tuple}));
377 }
else {jq.push_back(temp);}
381 std::stringstream ss;
382 ss <<
"<root main_tree_to_execute = \"MainTree\">\n";
383 ss <<
"<BehaviorTree ID=\"MainTree\">\n";
384 ss <<
"<Control ID=\"Parallel\" name=\"Agents\" success_threshold=\""<< 2 <<
"\" failure_threshold=\""<< 1 <<
"\">\n";
386 for (
const auto& s_r :
robots_){
388 auto a = task_but_different.at(s_r.first);
389 }
catch (std::out_of_range& oor){
392 ss <<
"<Control ID=\"Parallel_robot\" name=\""<< s_r.first <<
"\" success_threshold=\""<< task_but_different.at(s_r.first).size() <<
"\" failure_threshold=\""<< 1 <<
"\">\n";
393 for (
auto& p_obj: task_but_different.at(s_r.first)){
394 ss <<
"<SequenceStar name=\"root_sequence\">\n";
395 ss <<
"<Condition ID=\"PositionCondition\" name=\"Position_condition\"/>\n";
396 for(
int j = 0; j < std::get<2>(p_obj).size(); j++){
397 ss <<
"<Action ID=\"Execution\" name=\"Execution\"/>\n";
399 ss <<
"</SequenceStar>\n";
401 ss <<
"</Control>\n";
403 ss <<
"</Control>\n";
404 ss <<
"</BehaviorTree>\n";
407 std::cout << ss.str();
408 auto tree = factory.createTreeFromText(ss.str());
409 auto node_it = tree.nodes.begin();
411 for (
const auto& s_r :
robots_){
413 auto a = task_but_different.at(s_r.first);
414 }
catch (std::out_of_range& oor){
417 for (
auto& p_obj: task_but_different.at(s_r.first)){
418 while (node_it->get()->type() == BT::NodeType::CONTROL) ++node_it;
419 if(node_it->get()->type() == BT::NodeType::ACTION) ROS_INFO(
"Action");
420 if(node_it->get()->type() == BT::NodeType::CONDITION) ROS_INFO(
"Condition");
421 if(node_it->get()->type() == BT::NodeType::CONTROL) ROS_INFO(
"Control");
422 if(node_it->get()->type() == BT::NodeType::DECORATOR) ROS_INFO(
"Decorator");
424 if(
auto condition =
dynamic_cast<PositionCondition*
>(node_it->get())) {ROS_INFO(
"init Condition");condition->init(std::get<0>(p_obj), std::get<1>(p_obj),
psi_.get());++node_it;}
425 for(
int j = 0; j < std::get<2>(p_obj).size(); j++){
426 if(
auto execution =
dynamic_cast<Execution*
>(node_it->get())) {ROS_INFO(
"init Action");execution->init(&
executions_, s_r.second.get(), std::get<2>(p_obj)[j]);++node_it;}
431 BT::PublisherZMQ zmq(tree);
432 ros::Duration sleep(1);
433 BT::NodeStatus status = BT::NodeStatus::RUNNING;
434 auto t_start = std::chrono::high_resolution_clock::now();
435 while( status == BT::NodeStatus::RUNNING){
436 status = tree.tickRoot();
441 std::vector<std::thread> th;
442 moveit_msgs::PlanningScene ps_m;
445 for (
const auto& s_r :
robots_){
447 ROS_INFO(
"%s", s_r.first.c_str());
459 mergePS(ps_m, temp.second, s_r.second.get());
461 }
catch (std::out_of_range& oor){}
486 auto t_end = std::chrono::high_resolution_clock::now();
487 double elapsed_time_ms = std::chrono::duration<double, std::milli>(t_end-t_start).count();
488 ROS_INFO(
"%f", elapsed_time_ms);
491 for (
int i = 0; i < cd.size(); i ++){
492 std::stringstream ss;
495 moveit_msgs::CollisionObject item;
497 item.header.frame_id =
"world";
498 item.header.stamp = ros::Time::now();
499 item.primitives.resize(1);
500 item.primitives[0].type = item.primitives[0].BOX;
501 item.primitives[0].dimensions.resize(3);
502 item.primitives[0].dimensions[0] = cd[i].x_depth;
503 item.primitives[0].dimensions[1] = cd[i].y_width;
504 item.primitives[0].dimensions[2] = cd[i].z_heigth;
506 item.pose.position.x = cd[i].Pose.position.x;
507 item.pose.position.y = cd[i].Pose.position.y;
508 item.pose.position.z = cd[i].Pose.position.z;
509 item.pose.orientation.x = cd[i].Pose.orientation.x;
510 item.pose.orientation.y = cd[i].Pose.orientation.y;
511 item.pose.orientation.z = cd[i].Pose.orientation.z;
512 item.pose.orientation.w = cd[i].Pose.orientation.w;
513 item.operation = item.MOVE;
515 psi_->applyCollisionObject(item);
522 moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
523 acmt.entry_values.resize(
acm_.size());
526 for (
auto& a :
acm_){
527 acmt.entry_names.push_back(a.first);
528 acmt.entry_values[i].enabled = a.second;
541 ps_m.allowed_collision_matrix = acmt;
548 for (
auto ao : in.robot_state.attached_collision_objects) out.robot_state.attached_collision_objects.push_back(ao);
549 out.robot_state.is_diff |= in.robot_state.is_diff;
550 out.is_diff |= in.is_diff;
551 out.robot_state.joint_state.header = in.robot_state.joint_state.header;
552 out.robot_model_name =
"panda";
554 std::vector<std::string> links;
555 for (
auto link : mr->
mgi()->getLinkNames()) links.push_back(link);
556 links.push_back(mr->map()[
"left_finger"]);
557 links.push_back(mr->map()[
"right_finger"]);
558 links.push_back(mr->map()[
"hand_link"]);
559 links.push_back(mr->map()[
"base"]);
561 for (
auto link : links) {
562 for(
int i = 0; i < in.robot_state.joint_state.name.size(); i++){
563 if(link == in.robot_state.joint_state.name[i]){
564 out.robot_state.joint_state.effort.push_back(in.robot_state.joint_state.effort[i]);
565 out.robot_state.joint_state.position.push_back(in.robot_state.joint_state.position[i]);
566 out.robot_state.joint_state.velocity.push_back(in.robot_state.joint_state.velocity[i]);
570 for(
int i = 0; i < in.link_padding.size(); i++){
571 if(link == in.link_padding[i].link_name){
572 out.link_padding.push_back(in.link_padding[i]);
576 for(
int i = 0; i < in.link_scale.size(); i++){
577 if(link == in.link_scale[i].link_name){
578 out.link_scale.push_back(in.link_scale[i]);
586 mr.
mgi()->execute(rt);
590 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));
591 std::string support_surface1 =
"nichts";
592 std::string support_surface2 =
"nichts";
594 for (
const auto& s_r :
robots_){
596 std::bitset<3> panel_mask;
597 if(s_r.second->checkSingleObjectCollision(t, str, panel_mask)) support_surface1 = str;
598 if(s_r.second->checkSingleObjectCollision(target, str, panel_mask)) support_surface2= str;
601 ROS_INFO(
"ss1 %s", support_surface1.c_str());
602 ROS_INFO(
"ss2 %s", support_surface2.c_str());
605 const std::string
object = source.id;
606 moveit::task_constructor::Task task_;
608 std::string name =
"Pick&Place";
609 task_.stages()->setName(name + std::to_string(
static_cast<int>(ros::Time::now().toNSec())));
610 task_.loadRobotModel();
611 task_.setRobotModel(mr->
mgi()->getRobotModel());
614 task_.setProperty(
"group", mr->
name());
615 task_.setProperty(
"eef", mr->map()[
"eef_name"]);
616 task_.setProperty(
"hand", mr->map()[
"hand_group_name"]);
617 task_.setProperty(
"hand_grasping_frame", mr->map()[
"hand_frame"]);
618 task_.setProperty(
"ik_frame", mr->map()[
"hand_frame"]);
620 moveit::task_constructor::Stage* current_state_ptr =
nullptr;
622 auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>(
"current state");
623 auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>(
"applicability test", std::move(current_state));
624 applicability_filter->setPredicate([
object](
const moveit::task_constructor::SolutionBase& s, std::string& comment) {
625 if (s.start()->scene()->getCurrentState().hasAttachedBody(
object)) {
626 comment =
"object with id '" + object +
"' is already attached and cannot be picked";
632 current_state_ptr = applicability_filter.get();
633 task_.add(std::move(applicability_filter));
637 auto stage = std::make_unique< moveit::task_constructor::stages::MoveTo>(
"open hand",
sampling_planner_);
638 stage->setGroup(mr->map()[
"eef_name"]);
639 stage->setGoal(
"open");
640 task_.add(std::move(stage));
644 auto stage = std::make_unique< moveit::task_constructor::stages::Connect>(
645 "move to pick", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->
name(),
sampling_planner_} });
646 stage->setTimeout(5.0);
647 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
648 task_.add(std::move(stage));
651 moveit::task_constructor::Stage* attach_object_stage =
nullptr;
653 auto grasp = std::make_unique<moveit::task_constructor::SerialContainer>(
"pick object");
654 task_.properties().exposeTo(grasp->properties(), {
"eef",
"hand",
"group",
"ik_frame" });
655 grasp->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"eef",
"hand",
"group",
"ik_frame" });
659 auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>(
"approach object",
cartesian_planner_);
660 stage->properties().set(
"marker_ns",
"approach_object");
661 stage->properties().set(
"link", mr->map()[
"hand_frame"]);
662 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"group" });
663 stage->setMinMaxDistance(0.07, 0.2);
666 geometry_msgs::Vector3Stamped vec;
667 vec.header.frame_id = mr->map()[
"hand_frame"];
669 stage->setDirection(vec);
670 grasp->insert(std::move(stage));
675 auto stage = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>(
"generate grasp pose");
676 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
677 stage->properties().set(
"marker_ns",
"grasp_pose");
678 stage->setPreGraspPose(
"open");
679 stage->setObject(
object);
680 stage->setAngleDelta(M_PI / 2);
681 stage->setMonitoredStage(current_state_ptr);
684 Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
685 Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
686 Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
687 Eigen::Translation3d trans(0.1f, 0, 0);
688 Eigen::Isometry3d ik = eigen * trans;
690 auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>(
"grasp pose IK", std::move(stage));
691 wrapper->setMaxIKSolutions(8);
692 wrapper->setMinSolutionDistance(1.0);
693 wrapper->setIKFrame(ik, mr->map()[
"hand_frame"]);
694 wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"eef",
"group" });
695 wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, {
"target_pose" });
696 grasp->insert(std::move(wrapper));
700 auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>(
"allow collision (hand,object)");
701 stage->allowCollisions(
702 object, task_.getRobotModel()->getJointModelGroup(mr->map()[
"eef_name"])->getLinkModelNamesWithCollisionGeometry(),
704 grasp->insert(std::move(stage));
708 auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>(
"close hand",
sampling_planner_);
709 stage->setGroup(mr->map()[
"eef_name"]);
710 stage->setGoal(
"close");
711 grasp->insert(std::move(stage));
715 auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>(
"attach object");
716 stage->attachObject(
object, mr->map()[
"hand_frame"]);
717 attach_object_stage = stage.get();
718 grasp->insert(std::move(stage));
722 auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>(
"allow collision (object,support)");
723 stage->allowCollisions({
object }, support_surface1,
true);
724 grasp->insert(std::move(stage));
728 auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>(
"lift object",
cartesian_planner_);
729 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"group" });
730 stage->setMinMaxDistance(0.1, 0.2);
731 stage->setIKFrame(mr->map()[
"hand_frame"]);
732 stage->properties().set(
"marker_ns",
"lift_object");
735 geometry_msgs::Vector3Stamped vec;
736 vec.header.frame_id =
"world";
738 stage->setDirection(vec);
739 grasp->insert(std::move(stage));
743 auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>(
"forbid collision (object,surface)");
744 stage->allowCollisions({
object }, support_surface1,
false);
745 grasp->insert(std::move(stage));
749 task_.add(std::move(grasp));
753 auto stage = std::make_unique<moveit::task_constructor::stages::Connect>(
754 "move to place", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->
name(),
sampling_planner_ } });
755 stage->setTimeout(5.0);
756 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
757 task_.add(std::move(stage));
761 auto place = std::make_unique<moveit::task_constructor::SerialContainer>(
"place object");
762 task_.properties().exposeTo(place->properties(), {
"eef",
"hand",
"group" });
763 place->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"eef",
"hand",
"group" });
766 auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>(
"allow cokbkmomsurface)");
767 stage->allowCollisions( {
object} , support_surface2,
true);
768 place->insert(std::move(stage));
773 auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>(
"lower object",
cartesian_planner_);
774 stage->properties().set(
"marker_ns",
"lower_object");
775 stage->properties().set(
"link", mr->map()[
"hand_frame"]);
776 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"group" });
777 stage->setMinMaxDistance(.03, .13);
780 geometry_msgs::Vector3Stamped vec;
781 vec.header.frame_id =
"world";
783 stage->setDirection(vec);
784 place->insert(std::move(stage));
789 auto stage = std::make_unique<moveit::task_constructor::stages::GeneratePlacePose>(
"generate place pose");
790 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"ik_frame" });
791 stage->properties().set(
"marker_ns",
"place_pose");
792 stage->setObject(
object);
795 geometry_msgs::PoseStamped p;
796 p.header.frame_id =
"world";
797 p.pose.position.x = target.getOrigin().getX();
798 p.pose.position.y = target.getOrigin().getY();
799 p.pose.position.z = target.getOrigin().getZ();
800 p.pose.orientation.x = target.getRotation().getX();
801 p.pose.orientation.y = target.getRotation().getY();
802 p.pose.orientation.z = target.getRotation().getZ();
803 p.pose.orientation.w = target.getRotation().getW();
808 stage->setMonitoredStage(attach_object_stage);
811 Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
812 Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
813 Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
814 Eigen::Translation3d trans(0.1f, 0, 0);
815 Eigen::Isometry3d ik = eigen * trans;
816 auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>(
"place pose IK", std::move(stage));
817 wrapper->setMaxIKSolutions(2);
818 wrapper->setIKFrame(ik, mr->map()[
"hand_frame"]);
819 wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"eef",
"group" });
820 wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, {
"target_pose" });
821 place->insert(std::move(wrapper));
825 auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>(
"open hand",
sampling_planner_);
826 stage->setGroup(mr->map()[
"eef_name"]);
827 stage->setGoal(
"open");
828 place->insert(std::move(stage));
832 auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>(
"allow collision (hand,object)");
833 stage->allowCollisions(
834 object, task_.getRobotModel()->getJointModelGroup(mr->map()[
"eef_name"])->getLinkModelNamesWithCollisionGeometry(),
836 place->insert(std::move(stage));
840 auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>(
"detach object");
841 stage->detachObject(
object, mr->map()[
"hand_frame"]);
842 place->insert(std::move(stage));
846 auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>(
"retreat after place",
cartesian_planner_);
847 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"group" });
848 stage->setMinMaxDistance(.1, .2);
849 stage->setIKFrame(mr->map()[
"hand_frame"]);
850 stage->properties().set(
"marker_ns",
"retreat");
851 geometry_msgs::Vector3Stamped vec;
852 vec.header.frame_id = mr->map()[
"hand_frame"];
854 stage->setDirection(vec);
855 place->insert(std::move(stage));
859 auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>(
"close hand",
sampling_planner_);
860 stage->setGroup(mr->map()[
"eef_name"]);
861 stage->setGoal(
"close");
862 place->insert(std::move(stage));
866 task_.add(std::move(place));
870 auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>(
"move home",
sampling_planner_);
871 stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, {
"group" });
872 stage->setGoal(
"ready");
873 stage->restrictDirection(moveit::task_constructor::stages::MoveTo::FORWARD);
874 task_.add(std::move(stage));