netft_utils.cpp
Go to the documentation of this file.
1 
2 /*
3 Copyright (c) 2016, Los Alamos National Security, LLC
4 All rights reserved.
5 Copyright 2016. Los Alamos National Security, LLC. This software was produced under U.S. Government contract DE-AC52-06NA25396 for Los Alamos National Laboratory (LANL), which is operated by Los Alamos National Security, LLC for the U.S. Department of Energy. The U.S. Government has rights to use, reproduce, and distribute this software. NEITHER THE GOVERNMENT NOR LOS ALAMOS NATIONAL SECURITY, LLC MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY LIABILITY FOR THE USE OF THIS SOFTWARE. If software is modified to produce derivative works, such modified software should be clearly marked, so as not to confuse it with the version available from LANL.
6 
7 Additionally, redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
8 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
9 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
10 3. Neither the name of Los Alamos National Security, LLC, Los Alamos National Laboratory, LANL, the U.S. Government, nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
11 
12 THIS SOFTWARE IS PROVIDED BY LOS ALAMOS NATIONAL SECURITY, LLC AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL LOS ALAMOS NATIONAL SECURITY, LLC OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
13 
14 Authors: Alex von Sternberg, Andy Zelenak
15 */
16 
17 #include "netft_utils.h"
18 
19 int main(int argc, char **argv)
20 {
21  // Initialize the ros netft_utils_node
22  ros::init(argc, argv, "netft_utils_node");
23 
24  // Instantiate utils class
26  NetftUtils utils(n);
28  spinner.start();
29 
30  // Initialize utils
31  utils.initialize();
32 
33  // Set up user input
34  std::string world_frame;
35  std::string ft_frame;
36  double forceMaxU = 0.0;
37  double torqueMaxU = 0.0;
38  if(argc<3)
39  {
40  ROS_FATAL("You must pass in at least the world and ft frame as command line arguments. Argument options are [world frame, ft frame, max force, max torque]");
41  return 1;
42  }
43  else if(argc>=6)
44  {
45  ROS_FATAL("Too many arguments for netft_utils");
46  }
47  else
48  {
49  world_frame = argv[1];
50  ft_frame = argv[2];
51  if(argc>=4)
52  forceMaxU = atof(argv[3]);
53  if(5==argc)
54  torqueMaxU = atof(argv[4]);
55  }
56  utils.setUserInput(world_frame, ft_frame, forceMaxU, torqueMaxU);
57 
58  // Main ros loop
59  ros::Rate loop_rate(500);
60  //ros::Time last;
61  while(ros::ok())
62  {
63  utils.update();
64  loop_rate.sleep();
65  //ros::Time curr = ros::Time::now();
66  //ROS_INFO_STREAM("Loop time: " << curr.toSec()-last.toSec());
67  //last = curr;
68  }
69 
70  return 0;
71 }
72 
74  n(nh),
75  isFilterOn(false),
76  deltaTFilter(0.0),
77  cutoffFrequency(0.0),
78  newFilter(false),
79  isBiased(false),
80  isGravityBiased(false),
81  isNewBias(false),
82  isNewGravityBias(false),
83  cancel_count(MAX_CANCEL),
84  cancel_wait(MAX_WAIT),
85  forceMaxB(10.0),
86  torqueMaxB(0.8),
87  forceMaxU(50.0),
88  torqueMaxU(5.0),
89  payloadWeight(0.),
90  payloadLeverArm(0.)
91 {
92 }
93 
95 {
96  delete listener;
97  delete lp;
98 }
99 
101 {
102  //lp = new LPFilter(0.002,200,6);
103 
104  //Zero out the zero wrench
105  zero_wrench.wrench.force.x = 0.0;
106  zero_wrench.wrench.force.y = 0.0;
107  zero_wrench.wrench.force.z = 0.0;
108  zero_wrench.wrench.torque.x = 0.0;
109  zero_wrench.wrench.torque.y = 0.0;
110  zero_wrench.wrench.torque.z = 0.0;
111 
112  //Initialize cancel message
113  cancel_msg.toCancel = false;
114 
115  //Listen to the transfomation from the ft sensor to world frame.
117 
118  //Subscribe to the NetFT topic.
119  raw_data_sub = n.subscribe("netft_data",100, &NetftUtils::netftCallback, this);
120 
121  //Publish on the /netft_transformed_data topic. Queue up to 100000 data points
122  netft_raw_world_data_pub = n.advertise<geometry_msgs::WrenchStamped>("raw_world", 100000);
123  netft_world_data_pub = n.advertise<geometry_msgs::WrenchStamped>("transformed_world", 100000);
124  netft_tool_data_pub = n.advertise<geometry_msgs::WrenchStamped>("transformed_tool", 100000);
125  netft_cancel_pub = n.advertise<netft_utils::Cancel>("cancel", 100000);
126 
127  //Advertise bias and threshold services
130  set_max_service = n.advertiseService("set_max_values", &NetftUtils::setMax, this);
131  theshold_service = n.advertiseService("set_threshold", &NetftUtils::setThreshold, this);
135 }
136 
137 void NetftUtils::setUserInput(std::string world, std::string ft, double force, double torque)
138 {
139  world_frame = world;
140  ft_frame = ft;
141  if(force != 0.0)
142  {
143  forceMaxU = force;
144  }
145  if(torque != 0.0)
146  {
147  torqueMaxU = torque;
148  }
149 }
150 
152 {
153  // Check for a filter
154  if(newFilter)
155  {
156  delete lp;
158  newFilter = false;
159  }
160  // Look up transform from ft to world frame
161  tf::StampedTransform tempTransform;
162  try
163  {
165  listener->lookupTransform(world_frame, ft_frame, ros::Time(0), tempTransform);
166  }
167  catch (tf::TransformException ex)
168  {
169  ROS_ERROR("%s",ex.what());
170  }
171 
172  // Set translation to zero before updating value
173  tempTransform.setOrigin(tf::Vector3(0.0,0.0,0.0));
174  ft_to_world = tempTransform;
175 
176  checkMaxForce();
177 
178  // Publish transformed dat
183 
184  ros::spinOnce();
185 }
186 
187 void NetftUtils::copyWrench(geometry_msgs::WrenchStamped &in, geometry_msgs::WrenchStamped &out, geometry_msgs::WrenchStamped &bias)
188 {
189  out.header.stamp = in.header.stamp;
190  out.header.frame_id = in.header.frame_id;
191  out.wrench.force.x = in.wrench.force.x - bias.wrench.force.x;
192  out.wrench.force.y = in.wrench.force.y - bias.wrench.force.y;
193  out.wrench.force.z = in.wrench.force.z - bias.wrench.force.z;
194  out.wrench.torque.x = in.wrench.torque.x - bias.wrench.torque.x;
195  out.wrench.torque.y = in.wrench.torque.y - bias.wrench.torque.y;
196  out.wrench.torque.z = in.wrench.torque.z - bias.wrench.torque.z;
197 }
198 
199 void NetftUtils::applyThreshold(double &value, double thresh)
200 {
201  if(value <= thresh && value >= -thresh)
202  {
203  value = 0.0;
204  }
205 }
206 
207 void NetftUtils::transformFrame(geometry_msgs::WrenchStamped in_data, geometry_msgs::WrenchStamped &out_data, char target_frame)
208 {
209  tf::Vector3 tempF;
210  tf::Vector3 tempT;
211  tempF.setX(in_data.wrench.force.x);
212  tempF.setY(in_data.wrench.force.y);
213  tempF.setZ(in_data.wrench.force.z);
214  tempT.setX(in_data.wrench.torque.x);
215  tempT.setY(in_data.wrench.torque.y);
216  tempT.setZ(in_data.wrench.torque.z);
217  if(target_frame == 'w')
218  {
219  out_data.header.frame_id = world_frame;
220  tempF = ft_to_world * tempF;
221  tempT = ft_to_world * tempT;
222  }
223  else if(target_frame == 't')
224  {
225  out_data.header.frame_id = ft_frame;
226  tempF = ft_to_world.inverse() * tempF;
227  tempT = ft_to_world.inverse() * tempT;
228  }
229  out_data.header.stamp = in_data.header.stamp;
230  out_data.wrench.force.x = tempF.getX();
231  out_data.wrench.force.y = tempF.getY();
232  out_data.wrench.force.z = tempF.getZ();
233  out_data.wrench.torque.x = tempT.getX();
234  out_data.wrench.torque.y = tempT.getY();
235  out_data.wrench.torque.z = tempT.getZ();
236 }
237 
238 // Runs when a new datapoint comes in
239 void NetftUtils::netftCallback(const geometry_msgs::WrenchStamped::ConstPtr& data)
240 {
241  // Filter data
242  std::vector<double> tempData;
243  tempData.resize(6);
244  tempData.at(0) = -data->wrench.force.x;
245  tempData.at(1) = data->wrench.force.y;
246  tempData.at(2) = data->wrench.force.z;
247  tempData.at(3) = -data->wrench.torque.x;
248  tempData.at(4) = data->wrench.torque.y;
249  tempData.at(5) = data->wrench.torque.z;
250 
251  if(isFilterOn && !newFilter)
252  lp->update(tempData,tempData);
253 
254  // Copy tool frame data. apply negative to x data to follow right hand rule convention (ft raw data does not)
255  raw_data_tool.header.stamp = data->header.stamp;
256  raw_data_tool.header.frame_id = ft_frame;
257  raw_data_tool.wrench.force.x = tempData.at(0);
258  raw_data_tool.wrench.force.y = tempData.at(1);
259  raw_data_tool.wrench.force.z = tempData.at(2);
260  raw_data_tool.wrench.torque.x = tempData.at(3);
261  raw_data_tool.wrench.torque.y = tempData.at(4);
262  raw_data_tool.wrench.torque.z = tempData.at(5);
263 
264  // Copy in new netft data in tool frame and transform to world frame
266 
267  if (isGravityBiased) // Compensate for gravity. Assumes world Z-axis is up
268  {
269  // Gravity moment = (payloadLeverArm) cross (payload force) <== all in the sensor frame. Need to convert to world later
270  // Since it's assumed that the CoM of the payload is on the sensor's central axis, this calculation is simplified.
271  double gravMomentX = -payloadLeverArm*tf_data_tool.wrench.force.y;
272  double gravMomentY = payloadLeverArm*tf_data_tool.wrench.force.x;
273 
274  // Subtract the gravity torques from the previously-calculated wrench in the tool frame
275  tf_data_tool.wrench.torque.x = tf_data_tool.wrench.torque.x - gravMomentX;
276  tf_data_tool.wrench.torque.y = tf_data_tool.wrench.torque.y - gravMomentY;
277 
278  // Convert to world to account for the gravity force. Assumes world-Z is up.
279  //ROS_INFO_STREAM("gravity force in the world Z axis: "<< payloadWeight);
281  tf_data_world.wrench.force.z = tf_data_world.wrench.force.z - payloadWeight;
282 
283  // tf_data_world now accounts for gravity completely. Convert back to the tool frame to make that data available, too
285  }
286 
287  if (isBiased) // Apply the bias for a static sensor frame
288  {
289  // Get tool bias in world frame
290  geometry_msgs::WrenchStamped world_bias;
291  transformFrame(bias, world_bias, 'w');
292  // Add bias and apply threshold to get transformed data
294  }
295  else // Just pass the data straight through
296  {
299  }
300 
301  // Apply thresholds
302  applyThreshold(tf_data_world.wrench.force.x, threshold.wrench.force.x);
303  applyThreshold(tf_data_world.wrench.force.y, threshold.wrench.force.y);
304  applyThreshold(tf_data_world.wrench.force.z, threshold.wrench.force.z);
305  applyThreshold(tf_data_world.wrench.torque.x, threshold.wrench.torque.x);
306  applyThreshold(tf_data_world.wrench.torque.y, threshold.wrench.torque.y);
307  applyThreshold(tf_data_world.wrench.torque.z, threshold.wrench.torque.z);
308  applyThreshold(tf_data_tool.wrench.force.x, threshold.wrench.force.x);
309  applyThreshold(tf_data_tool.wrench.force.y, threshold.wrench.force.y);
310  applyThreshold(tf_data_tool.wrench.force.z, threshold.wrench.force.z);
311  applyThreshold(tf_data_tool.wrench.torque.x, threshold.wrench.torque.x);
312  applyThreshold(tf_data_tool.wrench.torque.y, threshold.wrench.torque.y);
313  applyThreshold(tf_data_tool.wrench.torque.z, threshold.wrench.torque.z);
314  //ROS_INFO_STREAM("Callback time: " << tf_data_tool.header.stamp.toSec()-ros::Time::now().toSec());
315 }
316 
317 // Set the readings from the sensor to zero at this instant and continue to apply the bias on future readings.
318 // This doesn't account for gravity.
319 // Useful when the sensor's orientation won't change.
320 // Run this method when the sensor is stationary to avoid inertial effects.
321 // Cannot bias the sensor if gravity compensation has already been applied.
322 bool NetftUtils::fixedOrientationBias(netft_utils::SetBias::Request &req, netft_utils::SetBias::Response &res)
323 {
324  if(req.toBias)
325  {
326  copyWrench(raw_data_tool, bias, zero_wrench); // Store the current wrench readings in the 'bias' variable, to be applied hereafter
327  if(req.forceMax >= 0.0001) // if forceMax was specified and > 0
328  forceMaxB = req.forceMax;
329  if(req.torqueMax >= 0.0001)
330  torqueMaxB = req.torqueMax; // if torqueMax was specified and > 0
331 
332  isNewBias = true;
333  isBiased = true;
334  }
335  else
336  {
337  copyWrench(zero_wrench, bias, zero_wrench); // Clear the stored bias if the argument was false
338  }
339 
340  res.success = true;
341 
342  return true;
343 }
344 
345 // Calculate the payload's mass and center of mass so gravity can be compensated for, even as the sensor changes orientation.
346 // It's assumed that the payload's center of mass is located on the sensor's central access.
347 // Run this method when the sensor is stationary to avoid inertial effects.
348 // Cannot do gravity compensation if sensor has already been biased.
349 bool NetftUtils::compensateForGravity(netft_utils::SetBias::Request &req, netft_utils::SetBias::Response &res)
350 {
351 
352  if(req.toBias)
353  {
354  if (isBiased)
355  {
356  ROS_ERROR("Cannot compensate for gravity if the sensor has already been biased, i.e. useful data was wiped out");
357  res.success = false;
358  return false;
359  }
360  else // Cannot compensate for gravity if the sensor has already been biased, i.e. useful data was wiped out
361  {
362  // Get the weight of the payload. Assumes the world Z axis is up.
363  payloadWeight = raw_data_world.wrench.force.z;
364 
365  // Calculate the z-coordinate of the payload's center of mass, in the sensor frame.
366  // It's assumed that the x- and y-coordinates are zero.
367  // This is a lever arm.
368  payloadLeverArm = raw_data_tool.wrench.torque.y/raw_data_tool.wrench.force.x;
369 
370  isNewGravityBias = true;
371  isGravityBiased = true;
372  }
373  }
374 
375  res.success = true;
376  return true;
377 }
378 
379 bool NetftUtils::setFilter(netft_utils::SetFilter::Request &req, netft_utils::SetFilter::Response &res)
380 {
381  if(req.toFilter)
382  {
383  newFilter = true;
384  isFilterOn = true;
385  deltaTFilter = req.deltaT;
386  cutoffFrequency = req.cutoffFrequency;
387  }
388  else
389  {
390  isFilterOn = false;
391  }
392 
393  return true;
394 }
395 
396 bool NetftUtils::setMax(netft_utils::SetMax::Request &req, netft_utils::SetMax::Response &res)
397 {
398  if(req.forceMax >= 0.0001)
399  forceMaxU = req.forceMax;
400  if(req.torqueMax >= 0.0001)
401  torqueMaxU = req.torqueMax;
402 
403  res.success = true;
404  return true;
405 }
406 
407 bool NetftUtils::setWeightBias(netft_utils::SetBias::Request &req, netft_utils::SetBias::Response &res)
408 {
409  if(req.toBias)
410  {
412  }
413  else
414  {
416  }
417  res.success = true;
418 
419  return true;
420 }
421 
422 bool NetftUtils::getWeight(netft_utils::GetDouble::Request &req, netft_utils::GetDouble::Response &res)
423 {
424  geometry_msgs::WrenchStamped carried_weight;
425  copyWrench(raw_data_tool, carried_weight, weight_bias);
426  res.weight = pow((pow(carried_weight.wrench.force.x, 2.0) + pow(carried_weight.wrench.force.y, 2.0) + pow(carried_weight.wrench.force.z, 2.0)), 0.5)/9.81*1000;
427 
428  return true;
429 }
430 
431 bool NetftUtils::setThreshold(netft_utils::SetThreshold::Request &req, netft_utils::SetThreshold::Response &res)
432 {
433  threshold.wrench.force.x = req.data.wrench.force.x;
434  threshold.wrench.force.y = req.data.wrench.force.y;
435  threshold.wrench.force.z = req.data.wrench.force.z;
436  threshold.wrench.torque.x = req.data.wrench.torque.x;
437  threshold.wrench.torque.y = req.data.wrench.torque.y;
438  threshold.wrench.torque.z = req.data.wrench.torque.z;
439 
440  res.success = true;
441 
442  return true;
443 }
444 
446 {
447  double fMag = pow((pow(tf_data_tool.wrench.force.x, 2.0) + pow(tf_data_tool.wrench.force.y, 2.0) + pow(tf_data_tool.wrench.force.z, 2.0)), 0.5);
448  double tMag = pow((pow(tf_data_tool.wrench.torque.x, 2.0) + pow(tf_data_tool.wrench.torque.y, 2.0) + pow(tf_data_tool.wrench.torque.z, 2.0)), 0.5);
449  double fMax;
450  double tMax;
451  if(isBiased && !isNewBias)
452  {
453  fMax = forceMaxB;
454  tMax = torqueMaxB;
455  }
456  else
457  {
458  if(isBiased && isNewBias)
459  {
460  isNewBias = false;
461  }
462 
463  fMax = forceMaxU; //50.0;
464  tMax = torqueMaxU; //5.0;
465  }
466 
467  // If max FT exceeded, send cancel unless we have just sent it MAX_CANCEL times
468  //ROS_INFO("FMAG: %f TMAG: %f", fMag, tMag);
469  if((fabs(fMag) > fMax || fabs(tMag) > tMax) && cancel_count > 0)
470  {
471  cancel_msg.toCancel = true;
472  //ROS_INFO("Force torque violation. Canceling move.");
473  ROS_INFO("FMAG: %f FMAX: %f TMAG:%f TMAX: %f count: %d wait: %d", fMag, fMax, tMag, tMax, cancel_count, cancel_wait);
474  cancel_count-=1;
475  }
476  // If we have sent cancel MAX_CANCEL times, don't send cancel again for MAX_WAIT cycles
477  else if(cancel_count == 0 && cancel_wait > 0 && cancel_wait <= MAX_WAIT)
478  {
479  cancel_msg.toCancel = false;
480  cancel_wait-=1;
481  }
482  // If we have just finished waiting MAX_WAIT times, or the max force is no longer exceeded, reset cancel_count and cancel_wait
483  else if(cancel_wait == 0 || !(fabs(fMag) > fMax || fabs(tMag) > tMax))
484  {
485  cancel_msg.toCancel = false;
488  }
489 }
490 
std::string world_frame
Definition: netft_utils.h:46
geometry_msgs::WrenchStamped tf_data_world
Definition: netft_utils.h:54
ros::ServiceServer set_max_service
Definition: netft_utils.h:92
NetftUtils(ros::NodeHandle nh)
Definition: netft_utils.cpp:73
#define ROS_FATAL(...)
void publish(const boost::shared_ptr< M > &message) const
TFSIMD_FORCE_INLINE void setX(tfScalar x)
bool update(std::vector< double > input, std::vector< double > &output)
Definition: lpfilter.cpp:66
TFSIMD_FORCE_INLINE void setY(tfScalar y)
bool newFilter
Definition: netft_utils.h:41
bool setThreshold(netft_utils::SetThreshold::Request &req, netft_utils::SetThreshold::Response &res)
int cancel_wait
Definition: netft_utils.h:72
ros::Publisher netft_cancel_pub
Definition: netft_utils.h:85
geometry_msgs::WrenchStamped raw_data_tool
Definition: netft_utils.h:53
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
double payloadWeight
Definition: netft_utils.h:59
void checkMaxForce()
double torqueMaxU
Definition: netft_utils.h:76
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
void transformFrame(geometry_msgs::WrenchStamped in_data, geometry_msgs::WrenchStamped &out_data, char target_frame)
int main(int argc, char **argv)
Definition: netft_utils.cpp:19
bool fixedOrientationBias(netft_utils::SetBias::Request &req, netft_utils::SetBias::Response &res)
TFSIMD_FORCE_INLINE const tfScalar & getY() const
ros::Publisher netft_tool_data_pub
Definition: netft_utils.h:84
geometry_msgs::WrenchStamped threshold
Definition: netft_utils.h:57
tf::TransformListener * listener
Definition: netft_utils.h:44
double cutoffFrequency
Definition: netft_utils.h:40
bool setFilter(netft_utils::SetFilter::Request &req, netft_utils::SetFilter::Response &res)
ros::ServiceServer gravity_comp_service
Definition: netft_utils.h:91
ros::NodeHandle n
Definition: netft_utils.h:34
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
double torqueMaxB
Definition: netft_utils.h:74
geometry_msgs::WrenchStamped zero_wrench
Definition: netft_utils.h:56
bool isNewBias
Definition: netft_utils.h:63
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
ros::Publisher netft_world_data_pub
Definition: netft_utils.h:83
double forceMaxB
Definition: netft_utils.h:73
void spinner()
LPFilter * lp
Definition: netft_utils.h:37
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
void applyThreshold(double &value, double thresh)
TFSIMD_FORCE_INLINE const tfScalar & x() const
geometry_msgs::WrenchStamped tf_data_tool
Definition: netft_utils.h:55
bool isBiased
Definition: netft_utils.h:62
ros::Publisher netft_raw_world_data_pub
Definition: netft_utils.h:82
std::string ft_frame
Definition: netft_utils.h:47
bool getWeight(netft_utils::GetDouble::Request &req, netft_utils::GetDouble::Response &res)
void netftCallback(const geometry_msgs::WrenchStamped::ConstPtr &data)
ros::ServiceServer bias_service
Definition: netft_utils.h:90
ros::ServiceServer theshold_service
Definition: netft_utils.h:93
bool setMax(netft_utils::SetMax::Request &req, netft_utils::SetMax::Response &res)
#define ROS_INFO(...)
int cancel_count
Definition: netft_utils.h:71
double forceMaxU
Definition: netft_utils.h:75
ROSCPP_DECL bool ok()
bool isGravityBiased
Definition: netft_utils.h:65
bool compensateForGravity(netft_utils::SetBias::Request &req, netft_utils::SetBias::Response &res)
Transform inverse() const
ros::Subscriber raw_data_sub
Definition: netft_utils.h:79
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
geometry_msgs::WrenchStamped weight_bias
Definition: netft_utils.h:51
static const int MAX_CANCEL
Definition: netft_utils.h:69
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
ros::ServiceServer weight_bias_service
Definition: netft_utils.h:94
bool sleep()
double payloadLeverArm
Definition: netft_utils.h:60
ros::ServiceServer filter_service
Definition: netft_utils.h:96
geometry_msgs::WrenchStamped raw_data_world
Definition: netft_utils.h:52
void copyWrench(geometry_msgs::WrenchStamped &in, geometry_msgs::WrenchStamped &out, geometry_msgs::WrenchStamped &bias)
void setUserInput(std::string world, std::string ft, double force, double torque)
double deltaTFilter
Definition: netft_utils.h:39
static const int MAX_WAIT
Definition: netft_utils.h:70
TFSIMD_FORCE_INLINE const tfScalar & getX() const
ros::ServiceServer get_weight_service
Definition: netft_utils.h:95
void initialize()
tf::StampedTransform ft_to_world
Definition: netft_utils.h:45
ROSCPP_DECL void spinOnce()
bool isNewGravityBias
Definition: netft_utils.h:64
#define ROS_ERROR(...)
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
bool isFilterOn
Definition: netft_utils.h:38
geometry_msgs::WrenchStamped bias
Definition: netft_utils.h:50
netft_utils::Cancel cancel_msg
Definition: netft_utils.h:68
bool setWeightBias(netft_utils::SetBias::Request &req, netft_utils::SetBias::Response &res)
void update()


netft_utils
Author(s): Alex von Sternberg , Derek King, Andy Zelenak
autogenerated on Tue Mar 2 2021 03:15:08