panda_decorator.cpp
Go to the documentation of this file.
2 #include <regex>
3 
4 PandaDecorator::PandaDecorator(std::unique_ptr<AbstractRobot> next)
5  : AbstractRobotDecorator(std::move(next))
6  {
7  pattern_ = "panda_arm([0-9]+)";
9  }
10 
12  // panda root
13  next_->robotRootBounds().clear();
14  next_->robotRootBounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, 0.095f, 0))); // ++
15  next_->robotRootBounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, 0.095f, 0))); // +-
16  next_->robotRootBounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, -0.095f, 0)));
17  next_->robotRootBounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, -0.095f, 0)));
18 }
19 
21  mgi_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(next_->name());
22 
23  std::stringstream hand_n, ik_frame_n, name_n, base_n;
24 
25  std::regex panda_id("panda_arm([0-9]+)"), left_finger("left_finger"), right_finger("right_finger"), hand_link("hand_link");
26  std::smatch match;
27  std::regex_match(next_->name(), match, panda_id);
28 
29  hand_n << "hand_" << match[1];
30  ik_frame_n << "panda_" << match[1] << "_link8";
31  base_n << "base_" << match[1];
32 
33  mgi_hand_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(hand_n.str());
34 
35 
36  for (auto& link : mgi_hand_->getLinkNames()){
37  if (std::regex_match(link, match, left_finger)) map_.insert(std::pair<std::string, std::string>("left_finger", link));
38  if (std::regex_match(link, match, right_finger)) map_.insert(std::pair<std::string, std::string>("right_finger", link));
39  if (std::regex_match(link, match, hand_link)) map_.insert(std::pair<std::string, std::string>("hand_link", link));
40  }
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()));
45 }
PandaDecorator::spezifieRootBounds
void spezifieRootBounds() override
Panda Root spezififcation.
Definition: panda_decorator.cpp:11
AbstractRobotDecorator::next_
std::unique_ptr< AbstractRobot > next_
Abstract Robot which is mimiced.
Definition: abstract_robot_decorator.h:24
AbstractRobotDecorator::map_
std::map< std::string, std::string > map_
//!< Mapping of specific task constructor variables
Definition: abstract_robot_decorator.h:28
PandaDecorator::spezifieRobotGroups
void spezifieRobotGroups() override
Panda Mapping spezififcation.
Definition: panda_decorator.cpp:20
AbstractRobotDecorator::mgi_
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > mgi_
MoveGroup Interface for the robots arm.
Definition: abstract_robot_decorator.h:25
AbstractRobotDecorator::pattern_
std::string pattern_
Regexpattern.
Definition: abstract_robot_decorator.h:30
AbstractRobotDecorator::mgi_hand_
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > mgi_hand_
MoveGroup Interface for the Robots manipulator.
Definition: abstract_robot_decorator.h:26
PandaDecorator::PandaDecorator
PandaDecorator(std::unique_ptr< AbstractRobot > next)
Panda Decorator.
Definition: panda_decorator.cpp:4
AbstractRobotDecorator
Abstract Robot Decorator.
Definition: abstract_robot_decorator.h:22
panda_decorator.h


multi_cell_builder
Author(s): Matteo Anedda
autogenerated on Sun Apr 9 2023 23:59:51