netft_utils.h
Go to the documentation of this file.
1 #ifndef NET_FT_UTILS_H
2 #define NET_FT_UTILS_H
3 
4 #include "ros/ros.h"
5 #include "std_msgs/String.h"
7 #include "geometry_msgs/WrenchStamped.h"
8 #include "netft_utils/SetBias.h"
9 #include "netft_utils/SetMax.h"
10 #include "netft_utils/SetThreshold.h"
11 #include "netft_utils/SetToolData.h"
12 #include "netft_utils/SetFilter.h"
13 #include "netft_utils/GetDouble.h"
14 #include "netft_utils/Cancel.h"
15 #include "lpfilter.h"
16 #include <math.h>
17 
23 {
24 public:
26  ~NetftUtils();
27 
28  void initialize();
29  void setUserInput(std::string world, std::string ft, double force, double torque);
30  void update();
31 
32 private:
33  //Node handle
34  ros::NodeHandle n; // ROS node handle
35 
36  //LPFilter
37  LPFilter* lp; // Filter
38  bool isFilterOn;
39  double deltaTFilter;
41  bool newFilter;
42 
43  // Transform listener
45  tf::StampedTransform ft_to_world; // Transform from ft frame to world frame
46  std::string world_frame;
47  std::string ft_frame;
48 
49  // Wrenches used to hold force/torque and bias data
50  geometry_msgs::WrenchStamped bias; // Wrench containing the current bias data in tool frame
51  geometry_msgs::WrenchStamped weight_bias; // Wrench containing the bias at a measurement pose (to measure the weight)
52  geometry_msgs::WrenchStamped raw_data_world; // Wrench containing the current raw data from the netft sensor transformed into the world frame
53  geometry_msgs::WrenchStamped raw_data_tool; // Wrench containing the current raw data from the netft sensor in the tool frame
54  geometry_msgs::WrenchStamped tf_data_world; // Wrench containing the transformed (world frame) data with bias and threshold applied
55  geometry_msgs::WrenchStamped tf_data_tool; // Wrench containing the transformed (tool frame) data with bias and threshold applied
56  geometry_msgs::WrenchStamped zero_wrench; // Wrench of all zeros for convenience
57  geometry_msgs::WrenchStamped threshold; // Wrench containing thresholds
58 
59  double payloadWeight; // Used in gravity compensation
60  double payloadLeverArm; // Used in gravity compensation. The z-coordinate to payload CoM (in sensor's raw frame)
61 
62  bool isBiased; // True if sensor is biased
63  bool isNewBias; // True if sensor was biased this pass
64  bool isNewGravityBias; // True if gravity compensation was applied this pass
65  bool isGravityBiased; // True if gravity is compensated
66 
67  // Variables used to monitor FT violation and send a cancel move message
68  netft_utils::Cancel cancel_msg;
69  static const int MAX_CANCEL = 5; // Number of times to send cancel message when max force is exceeded
70  static const int MAX_WAIT = 100; // Number of cycles to wait after cancel message before sending again
71  int cancel_count; // Counter for times to send cancel message when max force is exceeded
72  int cancel_wait; // Counter of cycles to wait after cancel message before sending again
73  double forceMaxB; // Default max force limit to send cancel when FT is biased
74  double torqueMaxB; // Default max torque limit to send cancel when FT is biased
75  double forceMaxU; // Default max force limit to send cancel when FT is unbiased
76  double torqueMaxU; // Default max torque limit to send cancel when FT is unbiased
77 
78  // ROS subscribers
80 
81  // ROS publishers
86 
88  // ROS services
97 
99  // Callback methods
101 
102  // Runs when a new datapoint comes in
103  void netftCallback(const geometry_msgs::WrenchStamped::ConstPtr& data);
104 
105  // Set the readings from the sensor to zero at this instant and continue to apply the bias on future readings.
106  // This doesn't account for gravity i.e. it will not change if the sensor's orientation changes.
107  // Run this method when the sensor is stationary to avoid inertial effects.
108  bool fixedOrientationBias(netft_utils::SetBias::Request &req, netft_utils::SetBias::Response &res);
109 
110  // Set the readings from the sensor to zero at this instant.
111  // Calculate the payload's mass and center of mass so gravity can be compensated for, even as the sensor changes orientation.
112  // It's assumed that the payload's center of mass is located on the sensor's central access.
113  // Run this method when the sensor is stationary to avoid inertial effects.
114  // It assumes the Z-axis of the World tf frame is up.
115  bool compensateForGravity(netft_utils::SetBias::Request &req, netft_utils::SetBias::Response &res);
116 
117  bool setMax(netft_utils::SetMax::Request &req, netft_utils::SetMax::Response &res);
118  bool setWeightBias(netft_utils::SetBias::Request &req, netft_utils::SetBias::Response &res);
119  bool getWeight(netft_utils::GetDouble::Request &req, netft_utils::GetDouble::Response &res);
120  bool setThreshold(netft_utils::SetThreshold::Request &req, netft_utils::SetThreshold::Response &res);
121  bool setFilter(netft_utils::SetFilter::Request &req, netft_utils::SetFilter::Response &res);
122 
123  // Convenience methods
124  void copyWrench(geometry_msgs::WrenchStamped &in, geometry_msgs::WrenchStamped &out, geometry_msgs::WrenchStamped &bias);
125  void applyThreshold(double &value, double thresh);
126  void transformFrame(geometry_msgs::WrenchStamped in_data, geometry_msgs::WrenchStamped &out_data, char target_frame);
127  void checkMaxForce();
128 };
129 
130 #endif
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
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
double payloadWeight
Definition: netft_utils.h:59
void checkMaxForce()
double torqueMaxU
Definition: netft_utils.h:76
void transformFrame(geometry_msgs::WrenchStamped in_data, geometry_msgs::WrenchStamped &out_data, char target_frame)
bool fixedOrientationBias(netft_utils::SetBias::Request &req, netft_utils::SetBias::Response &res)
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
bool setFilter(netft_utils::SetFilter::Request &req, netft_utils::SetFilter::Response &res)
double cutoffFrequency
Definition: netft_utils.h:40
ros::ServiceServer gravity_comp_service
Definition: netft_utils.h:91
ros::NodeHandle n
Definition: netft_utils.h:34
double torqueMaxB
Definition: netft_utils.h:74
geometry_msgs::WrenchStamped zero_wrench
Definition: netft_utils.h:56
bool isNewBias
Definition: netft_utils.h:63
ros::Publisher netft_world_data_pub
Definition: netft_utils.h:83
double forceMaxB
Definition: netft_utils.h:73
LPFilter * lp
Definition: netft_utils.h:37
void applyThreshold(double &value, double thresh)
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)
int cancel_count
Definition: netft_utils.h:71
double forceMaxU
Definition: netft_utils.h:75
bool isGravityBiased
Definition: netft_utils.h:65
bool compensateForGravity(netft_utils::SetBias::Request &req, netft_utils::SetBias::Response &res)
ros::Subscriber raw_data_sub
Definition: netft_utils.h:79
geometry_msgs::WrenchStamped weight_bias
Definition: netft_utils.h:51
static const int MAX_CANCEL
Definition: netft_utils.h:69
ros::ServiceServer weight_bias_service
Definition: netft_utils.h:94
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
ros::ServiceServer get_weight_service
Definition: netft_utils.h:95
void initialize()
tf::StampedTransform ft_to_world
Definition: netft_utils.h:45
bool isNewGravityBias
Definition: netft_utils.h:64
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