wrench_manager.cpp
Go to the documentation of this file.
2 
4 {
6 {
7  if (!nh_.getParam("wrench_manager/max_tf_attempts", max_tf_attempts_))
8  {
9  ROS_WARN(
10  "WrenchManager: Missing max_tf_attempts parameter, setting default");
11  max_tf_attempts_ = 5;
12  }
13 }
14 
16 
18  const std::string &end_effector, const std::string &sensor_frame,
19  const std::string &gripping_point_frame, const std::string &sensor_topic,
20  const std::string &calib_matrix_param)
21 {
22  if (sensor_frame_.find(end_effector) != sensor_frame_.end())
23  {
24  ROS_ERROR(
25  "Cannot initialize wrench subscriber for end-effector %s: already "
26  "initialized",
27  end_effector.c_str());
28  return false;
29  }
30 
31  // get rigid transform between sensor frame and arm gripping point
32  geometry_msgs::PoseStamped sensor_to_gripping_point;
33  sensor_to_gripping_point.header.frame_id = sensor_frame;
34  sensor_to_gripping_point.header.stamp = ros::Time(0);
35  sensor_to_gripping_point.pose.position.x = 0;
36  sensor_to_gripping_point.pose.position.y = 0;
37  sensor_to_gripping_point.pose.position.z = 0;
38  sensor_to_gripping_point.pose.orientation.x = 0;
39  sensor_to_gripping_point.pose.orientation.y = 0;
40  sensor_to_gripping_point.pose.orientation.z = 0;
41  sensor_to_gripping_point.pose.orientation.w = 1;
42 
43  int attempts;
44  for (attempts = 0; attempts < max_tf_attempts_; attempts++)
45  {
46  try
47  {
48  listener_.transformPose(gripping_point_frame, sensor_to_gripping_point,
49  sensor_to_gripping_point);
50  break;
51  }
52  catch (tf::TransformException ex)
53  {
54  ROS_WARN("TF exception in wrench manager: %s", ex.what());
55  }
56  ros::Duration(0.1).sleep();
57  }
58 
59  if (attempts >= max_tf_attempts_)
60  {
61  ROS_ERROR(
62  "WrenchManager: could not find the transform between the sensor frame "
63  "%s and gripping point %s",
64  sensor_frame.c_str(), gripping_point_frame.c_str());
65  return false;
66  }
67 
68  Eigen::MatrixXd C;
69  if (!parser_.parseMatrixData(C, calib_matrix_param, nh_))
70  {
71  ROS_ERROR(
72  "WrenchManager: missing force torque sensor calibration matrix "
73  "parameter %s",
74  calib_matrix_param.c_str());
75  return false;
76  }
77 
78  if (C.cols() != 6 || C.rows() != 6)
79  {
80  ROS_ERROR("WrenchManager: calibration matrix must be 6x6. Got %ldx%ld",
81  C.rows(), C.cols());
82  return false;
83  }
84 
85  // Everything is ok, can add new comm.
86  KDL::Frame sensor_to_gripping_point_kdl;
87  calibration_matrix_[end_effector] = C;
88  tf::poseMsgToKDL(sensor_to_gripping_point.pose, sensor_to_gripping_point_kdl);
89  sensor_frame_[end_effector] = sensor_frame;
90  sensor_to_gripping_point_[end_effector] = sensor_to_gripping_point_kdl;
91  measured_wrench_[end_effector] = KDL::Wrench::Zero();
92  ft_sub_[end_effector] =
93  nh_.subscribe(sensor_topic, 1, &WrenchManager::forceTorqueCB, this);
94  gripping_frame_[end_effector] = gripping_point_frame;
95  processed_ft_pub_[end_effector] = nh_.advertise<geometry_msgs::WrenchStamped>(
96  sensor_topic + "_converted", 1);
97 
98  return true;
99 }
100 
102  const std::string &end_effector, Eigen::Matrix<double, 6, 1> &wrench) const
103 {
104  if (sensor_frame_.find(end_effector) == sensor_frame_.end())
105  {
106  return false;
107  }
108 
109  KDL::Wrench wrench_kdl;
110  geometry_msgs::WrenchStamped temp_wrench;
111  wrench_kdl = sensor_to_gripping_point_.at(end_effector) *
112  measured_wrench_.at(end_effector);
113  tf::wrenchKDLToEigen(wrench_kdl, wrench);
114 
115  // publish processed wrench to facilitate debugging
116  tf::wrenchKDLToMsg(wrench_kdl, temp_wrench.wrench);
117  temp_wrench.header.frame_id = gripping_frame_.at(end_effector);
118  temp_wrench.header.stamp = ros::Time::now();
119  processed_ft_pub_.at(end_effector).publish(temp_wrench);
120 
121  return true;
122 }
123 
125  const std::string &end_effector, Eigen::Matrix<double, 6, 1> &wrench) const
126 {
127  if (sensor_frame_.find(end_effector) == sensor_frame_.end())
128  {
129  return false;
130  }
131 
132  tf::wrenchKDLToEigen(measured_wrench_.at(end_effector), wrench);
133 
134  return true;
135 }
136 
138  const geometry_msgs::WrenchStamped::ConstPtr &msg)
139 {
140  std::string eef;
141  for (auto const &it : sensor_frame_)
142  {
143  if (msg->header.frame_id == it.second)
144  {
145  eef = it.first;
146  break;
147  }
148  }
149 
150  if (eef.empty())
151  {
152  ROS_ERROR(
153  "WrenchManager: got wrench message from sensor at frame %s, which was "
154  "not configured in the wrench manager",
155  msg->header.frame_id.c_str());
156  return;
157  }
158 
159  // apply computed sensor intrinsic calibration
160  Eigen::Matrix<double, 6, 1> wrench_eig;
161  tf::wrenchMsgToEigen(msg->wrench, wrench_eig);
162  wrench_eig = calibration_matrix_.at(eef) * wrench_eig;
163  tf::wrenchEigenToKDL(wrench_eig, measured_wrench_.at(eef));
164 }
165 
166 bool setWrenchManager(const ArmInfo &arm_info, WrenchManager &manager)
167 {
168  if (arm_info.has_ft_sensor)
169  {
170  if (!manager.initializeWrenchComm(
171  arm_info.kdl_eef_frame, arm_info.sensor_frame,
172  arm_info.gripping_frame, arm_info.sensor_topic,
173  arm_info.name + "/sensor_calib"))
174  {
175  return false;
176  }
177 
178  ROS_DEBUG("WrenchManager: successfully initialized wrench comms for arm %s",
179  arm_info.name.c_str());
180  }
181  else
182  {
183  ROS_WARN("WrenchManager: end-effector %s has no F/T sensor.",
184  arm_info.kdl_eef_frame.c_str());
185  }
186 
187  return true;
188 }
189 
190 bool setWrenchManager(const ArmInfo &arm_info,
191  std::shared_ptr<WrenchManager> &manager)
192 {
193  return setWrenchManager(arm_info, *manager);
194 }
195 } // namespace generic_control_toolbox
std::map< std::string, ros::Subscriber > ft_sub_
bool setWrenchManager(const ArmInfo &arm_info, WrenchManager &manager)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::map< std::string, KDL::Frame > sensor_to_gripping_point_
bool sleep() const
bool wrenchAtGrippingPoint(const std::string &end_effector, Eigen::Matrix< double, 6, 1 > &wrench) const
std::map< std::string, std::string > gripping_frame_
WrenchManager(ros::NodeHandle nh_=ros::NodeHandle("~"))
#define ROS_WARN(...)
std::map< std::string, std::string > sensor_frame_
void wrenchKDLToEigen(const KDL::Wrench &k, Eigen::Matrix< double, 6, 1 > &e)
void forceTorqueCB(const geometry_msgs::WrenchStamped::ConstPtr &msg)
bool wrenchAtSensorPoint(const std::string &end_effector, Eigen::Matrix< double, 6, 1 > &wrench) const
void wrenchMsgToEigen(const geometry_msgs::Wrench &m, Eigen::Matrix< double, 6, 1 > &e)
std::map< std::string, Eigen::Matrix< double, 6, 6 > > calibration_matrix_
std::map< std::string, KDL::Wrench > measured_wrench_
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
std::map< std::string, ros::Publisher > processed_ft_pub_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static bool parseMatrixData(Eigen::MatrixXd &M, const std::string param_name, const ros::NodeHandle &n)
static Wrench Zero()
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
bool getParam(const std::string &key, std::string &s) const
static Time now()
void wrenchKDLToMsg(const KDL::Wrench &k, geometry_msgs::Wrench &m)
#define ROS_ERROR(...)
void wrenchEigenToKDL(const Eigen::Matrix< double, 6, 1 > &e, KDL::Wrench &k)
bool initializeWrenchComm(const std::string &end_effector, const std::string &sensor_frame, const std::string &gripping_point_frame, const std::string &sensor_topic, const std::string &calib_matrix_param)
#define ROS_DEBUG(...)


generic_control_toolbox
Author(s): diogo
autogenerated on Thu Jun 6 2019 19:54:44