7 #include <moveit/planning_pipeline/planning_pipeline.h>
8 #include <gb_grasp/MapGenerator.h>
14 std::shared_ptr<moveit::planning_interface::MoveGroupInterface>
mgi_;
15 std::shared_ptr<moveit::planning_interface::MoveGroupInterface>
mgi_hand_;
17 std::map<std::string, std::string>
map_;
24 ,
mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>(
name)){
26 std::stringstream hand_n, ik_frame_n, name_n, base_n;
27 hand_n <<
"hand_" <<
name.back();
28 ik_frame_n <<
"panda_" <<
name.back() <<
"_link8";
29 base_n <<
"base_" <<
name.back();
30 mgi_hand_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(hand_n.str());
33 map_.insert(std::make_pair<std::string, std::string>(
"right_finger",
mgi_hand_->getLinkNames()[0].c_str()));
34 map_.insert(std::make_pair<std::string, std::string>(
"left_finger",
mgi_hand_->getLinkNames()[1].c_str()));
35 map_.insert(std::make_pair<std::string, std::string>(
"hand_link",
mgi_hand_->getLinkNames()[2].c_str()));
36 map_.insert(std::make_pair<std::string, std::string>(
"eef_name", hand_n.str()));
37 map_.insert(std::make_pair<std::string, std::string>(
"hand_frame", ik_frame_n.str()));
38 map_.insert(std::make_pair<std::string, std::string>(
"hand_group_name", hand_n.str()));
39 map_.insert(std::make_pair<std::string, std::string>(
"base", base_n.str()));
43 inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface>
mgi() {
return mgi_;}
44 inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface>
mgi_hand() {
return mgi_hand_;}
48 inline std::map<std::string, std::string>&
map(){
return map_;}