16 next_->robot_root_bounds().clear();
17 next_->robot_root_bounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, 0.095f, 0)));
18 next_->robot_root_bounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, 0.095f, 0)));
19 next_->robot_root_bounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, -0.095f, 0)));
20 next_->robot_root_bounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, -0.095f, 0)));
24 std::stringstream hand_n, ik_frame_n, name_n, base_n;
26 std::regex panda_id(
"panda_arm([0-9]+)"), left_finger(
"left_finger"), right_finger(
"right_finger"), hand_link(
"hand_link");
28 std::regex_match(
next_->name(), match, panda_id);
30 hand_n <<
"hand_" << match[1];
31 ik_frame_n <<
"panda_" << match[1] <<
"_link8";
32 base_n <<
"base_" << match[1];
34 for (
auto& link :
mgi_hand_->getLinkNames()){
35 if (std::regex_match(link, match, left_finger))
map_.insert(std::pair<std::string, std::string>(
"left_finger", link));
36 if (std::regex_match(link, match, right_finger))
map_.insert(std::pair<std::string, std::string>(
"right_finger", link));
37 if (std::regex_match(link, match, hand_link))
map_.insert(std::pair<std::string, std::string>(
"hand_link", link));
40 mgi_hand_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(hand_n.str());
42 map_.insert(std::make_pair<std::string, std::string>(
"eef_name", hand_n.str()));
43 map_.insert(std::make_pair<std::string, std::string>(
"hand_frame", ik_frame_n.str()));
44 map_.insert(std::make_pair<std::string, std::string>(
"hand_group_name", hand_n.str()));
45 map_.insert(std::make_pair<std::string, std::string>(
"base", base_n.str()));