15 next_->robotRootBounds().clear();
16 next_->robotRootBounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.0745f, 0.0745f, 0)));
17 next_->robotRootBounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.0745f, 0.0745f, 0)));
18 next_->robotRootBounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.0745f, -0.0745f, 0)));
19 next_->robotRootBounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.0745f, -0.0745f, 0)));
23 std::stringstream hand_n, ik_frame_n, name_n, base_n;
25 std::regex panda_id(
"panda_arm([0-9]+)"), left_finger(
"left_finger"), right_finger(
"right_finger"), hand_link(
"hand_link");
27 std::regex_match(
next_->name(), match, panda_id);
29 hand_n <<
"hand_" << match[1];
30 ik_frame_n <<
"panda_" << match[1] <<
"_link8";
31 base_n <<
"base_" << match[1];
33 for (
auto& link :
mgi_hand_->getLinkNames()){
34 if (std::regex_match(link, match, left_finger))
map_.insert(std::pair<std::string, std::string>(
"left_finger", link));
35 if (std::regex_match(link, match, right_finger))
map_.insert(std::pair<std::string, std::string>(
"right_finger", link));
36 if (std::regex_match(link, match, hand_link))
map_.insert(std::pair<std::string, std::string>(
"hand_link", link));
39 mgi_hand_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(hand_n.str());
41 map_.insert(std::make_pair<std::string, std::string>(
"eef_name", hand_n.str()));
42 map_.insert(std::make_pair<std::string, std::string>(
"hand_frame", ik_frame_n.str()));
43 map_.insert(std::make_pair<std::string, std::string>(
"hand_group_name", hand_n.str()));
44 map_.insert(std::make_pair<std::string, std::string>(
"base", base_n.str()));