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 {
17  for (auto sub_pair : ft_sub_)
18  {
19  sub_pair.second.shutdown();
20  }
21 }
22 
23 bool WrenchManager::setGrippingPoint(const std::string &end_effector,
24  const std::string &gripping_point_frame)
25 {
26  if (sensor_frame_.find(end_effector) == sensor_frame_.end())
27  {
28  ROS_ERROR("Cannot set gripping point for end-effector %s: not initialized",
29  end_effector.c_str());
30  return false;
31  }
32 
33  std::string sensor_frame = sensor_frame_[end_effector];
34  geometry_msgs::PoseStamped sensor_to_gripping_point;
35  sensor_to_gripping_point.header.frame_id = sensor_frame;
36  sensor_to_gripping_point.header.stamp = ros::Time(0);
37  sensor_to_gripping_point.pose.position.x = 0;
38  sensor_to_gripping_point.pose.position.y = 0;
39  sensor_to_gripping_point.pose.position.z = 0;
40  sensor_to_gripping_point.pose.orientation.x = 0;
41  sensor_to_gripping_point.pose.orientation.y = 0;
42  sensor_to_gripping_point.pose.orientation.z = 0;
43  sensor_to_gripping_point.pose.orientation.w = 1;
44 
45  int attempts;
46  for (attempts = 0; attempts < max_tf_attempts_; attempts++)
47  {
48  try
49  {
50  listener_.transformPose(gripping_point_frame, sensor_to_gripping_point,
51  sensor_to_gripping_point);
52  break;
53  }
54  catch (tf::TransformException ex)
55  {
56  ROS_WARN("TF exception in wrench manager: %s", ex.what());
57  }
58  ros::Duration(0.1).sleep();
59  }
60 
61  if (attempts >= max_tf_attempts_)
62  {
63  ROS_ERROR(
64  "WrenchManager: could not find the transform between the sensor frame "
65  "%s and gripping point %s",
66  sensor_frame.c_str(), gripping_point_frame.c_str());
67  return false;
68  }
69 
70  KDL::Frame sensor_to_gripping_point_kdl;
71  tf::poseMsgToKDL(sensor_to_gripping_point.pose, sensor_to_gripping_point_kdl);
72  sensor_to_gripping_point_[end_effector] = sensor_to_gripping_point_kdl;
73  gripping_frame_[end_effector] = gripping_point_frame;
74  return true;
75 }
76 
78  const std::string &end_effector, const std::string &sensor_frame,
79  const std::string &gripping_point_frame, const std::string &sensor_topic,
80  const std::string &calib_matrix_param)
81 {
82  if (sensor_frame_.find(end_effector) != sensor_frame_.end())
83  {
84  ROS_ERROR(
85  "Cannot initialize wrench subscriber for end-effector %s: already "
86  "initialized",
87  end_effector.c_str());
88  return false;
89  }
90 
91  sensor_frame_[end_effector] = sensor_frame;
92  // get rigid transform between sensor frame and arm gripping point
93  if (!setGrippingPoint(end_effector, gripping_point_frame))
94  {
95  sensor_frame_.erase(end_effector);
96  return false;
97  }
98 
99  Eigen::MatrixXd C;
100  if (!parser_.parseMatrixData(C, calib_matrix_param, nh_))
101  {
102  ROS_WARN(
103  "WrenchManager: missing force torque sensor calibration matrix "
104  "parameter %s. Setting default.",
105  calib_matrix_param.c_str());
106  C = Eigen::Matrix<double, 6, 6>::Identity();
107  }
108  else
109  {
110  if (C.cols() != 6 || C.rows() != 6)
111  {
112  ROS_ERROR("WrenchManager: calibration matrix must be 6x6. Got %ldx%ld",
113  C.rows(), C.cols());
114  sensor_frame_.erase(end_effector);
115  return false;
116  }
117  }
118 
119  // Everything is ok, can add new comm.
120  calibration_matrix_[end_effector] = C;
121  measured_wrench_[end_effector] = KDL::Wrench::Zero();
122  ft_sub_[end_effector] =
123  nh_.subscribe(sensor_topic, 1, &WrenchManager::forceTorqueCB, this);
124  processed_ft_pub_[end_effector] = nh_.advertise<geometry_msgs::WrenchStamped>(
125  sensor_topic + "_converted", 1);
126 
127  return true;
128 }
129 
131  const std::string &end_effector, Eigen::Matrix<double, 6, 1> &wrench) const
132 {
133  if (sensor_frame_.find(end_effector) == sensor_frame_.end())
134  {
135  return false;
136  }
137 
138  KDL::Wrench wrench_kdl;
139  geometry_msgs::WrenchStamped temp_wrench;
140  wrench_kdl = sensor_to_gripping_point_.at(end_effector) *
141  measured_wrench_.at(end_effector);
142  tf::wrenchKDLToEigen(wrench_kdl, wrench);
143 
144  // publish processed wrench to facilitate debugging
145  tf::wrenchKDLToMsg(wrench_kdl, temp_wrench.wrench);
146  temp_wrench.header.frame_id = gripping_frame_.at(end_effector);
147  temp_wrench.header.stamp = ros::Time::now();
148  processed_ft_pub_.at(end_effector).publish(temp_wrench);
149 
150  return true;
151 }
152 
154  const std::string &end_effector, Eigen::Matrix<double, 6, 1> &wrench) const
155 {
156  if (sensor_frame_.find(end_effector) == sensor_frame_.end())
157  {
158  return false;
159  }
160 
161  KDL::Wrench wrench_kdl;
162  geometry_msgs::WrenchStamped temp_wrench;
163  wrench_kdl = sensor_to_gripping_point_.at(end_effector).M *
164  measured_wrench_.at(end_effector);
165  tf::wrenchKDLToEigen(wrench_kdl, wrench);
166 
167  // publish processed wrench to facilitate debugging
168  tf::wrenchKDLToMsg(wrench_kdl, temp_wrench.wrench);
169  temp_wrench.header.frame_id = gripping_frame_.at(end_effector);
170  temp_wrench.header.stamp = ros::Time::now();
171  processed_ft_pub_.at(end_effector).publish(temp_wrench);
172 
173  return true;
174 }
175 
177  const std::string &end_effector, Eigen::Matrix<double, 6, 1> &wrench) const
178 {
179  if (sensor_frame_.find(end_effector) == sensor_frame_.end())
180  {
181  return false;
182  }
183 
184  tf::wrenchKDLToEigen(measured_wrench_.at(end_effector), wrench);
185 
186  return true;
187 }
188 
190  const geometry_msgs::WrenchStamped::ConstPtr &msg)
191 {
192  std::string eef;
193  for (auto const &it : sensor_frame_)
194  {
195  if (msg->header.frame_id == it.second)
196  {
197  eef = it.first;
198  break;
199  }
200  }
201 
202  if (eef.empty())
203  {
204  ROS_ERROR(
205  "WrenchManager: got wrench message from sensor at frame %s, which "
206  "was "
207  "not configured in the wrench manager",
208  msg->header.frame_id.c_str());
209  return;
210  }
211 
212  // apply computed sensor intrinsic calibration
213  Eigen::Matrix<double, 6, 1> wrench_eig;
214  tf::wrenchMsgToEigen(msg->wrench, wrench_eig);
215  wrench_eig = calibration_matrix_.at(eef) * wrench_eig;
216  tf::wrenchEigenToKDL(wrench_eig, measured_wrench_.at(eef));
217 }
218 
219 bool setWrenchManager(const ArmInfo &arm_info, WrenchManager &manager)
220 {
221  if (arm_info.has_ft_sensor)
222  {
223  if (!manager.initializeWrenchComm(
224  arm_info.kdl_eef_frame, arm_info.sensor_frame,
225  arm_info.gripping_frame, arm_info.sensor_topic,
226  arm_info.name + "/sensor_calib"))
227  {
228  return false;
229  }
230 
231  ROS_DEBUG("WrenchManager: successfully initialized wrench comms for arm %s",
232  arm_info.name.c_str());
233  }
234  else
235  {
236  ROS_WARN("WrenchManager: end-effector %s has no F/T sensor.",
237  arm_info.kdl_eef_frame.c_str());
238  }
239 
240  return true;
241 }
242 
243 bool setWrenchManager(const ArmInfo &arm_info,
244  std::shared_ptr<WrenchManager> &manager)
245 {
246  return setWrenchManager(arm_info, *manager);
247 }
248 } // namespace generic_control_toolbox
bool setGrippingPoint(const std::string &end_effector, const std::string &gripping_point_frame)
std::map< std::string, ros::Subscriber > ft_sub_
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
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 wrenchAtGrippingPoint(const std::string &end_effector, Eigen::Matrix< double, 6, 1 > &wrench) const
std::map< std::string, std::string > gripping_frame_
bool wrenchAtSensorPointInGraspFrame(const std::string &end_effector, Eigen::Matrix< double, 6, 1 > &wrench) const
#define ROS_ERROR(...)
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)
#define ROS_DEBUG(...)
bool getParam(const std::string &key, std::string &s) 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()
bool wrenchAtSensorPoint(const std::string &end_effector, Eigen::Matrix< double, 6, 1 > &wrench) const
static Time now()
bool sleep() const
void wrenchKDLToMsg(const KDL::Wrench &k, geometry_msgs::Wrench &m)
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)


generic_control_toolbox
Author(s): diogo
autogenerated on Mon Feb 28 2022 22:24:37