netft_utils.cpp
Go to the documentation of this file.
00001 
00002 /*
00003 Copyright (c) 2016, Los Alamos National Security, LLC
00004 All rights reserved.
00005 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.
00006 
00007 Additionally, redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
00008 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 
00009 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. 
00010 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. 
00011 
00012 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.
00013 
00014 Authors: Alex von Sternberg, Andy Zelenak
00015 */
00016 
00017 #include "netft_utils.h"
00018 
00019 int main(int argc, char **argv)
00020 {
00021   // Initialize the ros netft_utils_node
00022   ros::init(argc, argv, "netft_utils_node");
00023 
00024   // Instantiate utils class
00025   ros::NodeHandle n;
00026   NetftUtils utils(n);
00027   ros::AsyncSpinner spinner(1);
00028   spinner.start();
00029 
00030   // Initialize utils
00031   utils.initialize();
00032 
00033   // Set up user input
00034   std::string world_frame;
00035   std::string ft_frame;
00036   double forceMaxU = 0.0;
00037   double torqueMaxU = 0.0;
00038   if(argc<3)
00039   {
00040     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]");
00041     return 1;
00042   }
00043   else if(argc>=6)
00044   {
00045     ROS_FATAL("Too many arguments for netft_utils");
00046   }
00047   else
00048   {
00049     world_frame = argv[1];
00050     ft_frame = argv[2];
00051     if(argc>=4)
00052       forceMaxU = atof(argv[3]);
00053     if(5==argc)
00054       torqueMaxU = atof(argv[4]);
00055   }
00056   utils.setUserInput(world_frame, ft_frame, forceMaxU, torqueMaxU);
00057 
00058   // Main ros loop
00059   ros::Rate loop_rate(500);
00060   //ros::Time last;
00061   while(ros::ok())
00062   {
00063     utils.update();
00064     loop_rate.sleep();
00065     //ros::Time curr = ros::Time::now();
00066     //ROS_INFO_STREAM("Loop time: " <<  curr.toSec()-last.toSec());
00067     //last = curr;
00068   }
00069 
00070   return 0;
00071 }
00072 
00073 NetftUtils::NetftUtils(ros::NodeHandle nh) :
00074   n(nh),
00075   isFilterOn(false),
00076   deltaTFilter(0.0),
00077   cutoffFrequency(0.0),
00078   newFilter(false),
00079   isBiased(false),
00080   isGravityBiased(false),
00081   isNewBias(false),
00082   isNewGravityBias(false),
00083   cancel_count(MAX_CANCEL),
00084   cancel_wait(MAX_WAIT),
00085   forceMaxB(10.0),
00086   torqueMaxB(0.8),
00087   forceMaxU(50.0),
00088   torqueMaxU(5.0),
00089   payloadWeight(0.),
00090   payloadLeverArm(0.)
00091 {
00092 }
00093 
00094 NetftUtils::~NetftUtils()
00095 {
00096   delete listener;
00097   delete lp;
00098 }
00099 
00100 void NetftUtils::initialize()
00101 {
00102   //lp = new LPFilter(0.002,200,6);
00103   
00104   //Zero out the zero wrench
00105   zero_wrench.wrench.force.x = 0.0;
00106   zero_wrench.wrench.force.y = 0.0;
00107   zero_wrench.wrench.force.z = 0.0;
00108   zero_wrench.wrench.torque.x = 0.0;
00109   zero_wrench.wrench.torque.y = 0.0;
00110   zero_wrench.wrench.torque.z = 0.0;
00111 
00112   //Initialize cancel message
00113   cancel_msg.toCancel = false;
00114 
00115   //Listen to the transfomation from the ft sensor to world frame.
00116   listener = new tf::TransformListener(ros::Duration(300));
00117 
00118   //Subscribe to the NetFT topic.
00119   raw_data_sub = n.subscribe("netft_data",100, &NetftUtils::netftCallback, this);
00120 
00121   //Publish on the /netft_transformed_data topic. Queue up to 100000 data points
00122   netft_raw_world_data_pub = n.advertise<geometry_msgs::WrenchStamped>("raw_world", 100000);
00123   netft_world_data_pub = n.advertise<geometry_msgs::WrenchStamped>("transformed_world", 100000);
00124   netft_tool_data_pub = n.advertise<geometry_msgs::WrenchStamped>("transformed_tool", 100000);
00125   netft_cancel_pub = n.advertise<netft_utils::Cancel>("cancel", 100000);
00126 
00127   //Advertise bias and threshold services
00128   bias_service = n.advertiseService("bias", &NetftUtils::fixedOrientationBias, this);
00129   gravity_comp_service = n.advertiseService("gravity_comp", &NetftUtils::compensateForGravity, this);
00130   set_max_service = n.advertiseService("set_max_values", &NetftUtils::setMax, this);
00131   theshold_service = n.advertiseService("set_threshold", &NetftUtils::setThreshold, this);
00132   weight_bias_service = n.advertiseService("set_weight_bias", &NetftUtils::setWeightBias, this);
00133   get_weight_service = n.advertiseService("get_weight", &NetftUtils::getWeight, this);
00134   filter_service = n.advertiseService("filter", &NetftUtils::setFilter, this);
00135 }
00136 
00137 void NetftUtils::setUserInput(std::string world, std::string ft, double force, double torque)
00138 {
00139   world_frame = world;
00140   ft_frame = ft;
00141   if(force != 0.0)
00142   {
00143     forceMaxU = force;
00144   }
00145   if(torque != 0.0)
00146   {
00147     torqueMaxU = torque;
00148   }
00149 }
00150 
00151 void NetftUtils::update()
00152 {
00153   // Check for a filter
00154   if(newFilter)
00155   {
00156     delete lp;
00157     lp = new LPFilter(deltaTFilter,cutoffFrequency,6);
00158     newFilter = false;
00159   }
00160   // Look up transform from ft to world frame
00161   tf::StampedTransform tempTransform;
00162   try
00163   {
00164     listener->waitForTransform(world_frame, ft_frame, ros::Time(0), ros::Duration(1.0));
00165     listener->lookupTransform(world_frame, ft_frame, ros::Time(0), tempTransform);
00166   }
00167   catch (tf::TransformException ex)
00168   {
00169     ROS_ERROR("%s",ex.what());
00170   }
00171 
00172   // Set translation to zero before updating value
00173   tempTransform.setOrigin(tf::Vector3(0.0,0.0,0.0));
00174   ft_to_world = tempTransform;
00175 
00176   checkMaxForce();
00177 
00178   // Publish transformed dat
00179   netft_raw_world_data_pub.publish( raw_data_world );
00180   netft_world_data_pub.publish( tf_data_world );
00181   netft_tool_data_pub.publish( tf_data_tool );
00182   netft_cancel_pub.publish( cancel_msg );
00183 
00184   ros::spinOnce();
00185 }
00186 
00187 void NetftUtils::copyWrench(geometry_msgs::WrenchStamped &in, geometry_msgs::WrenchStamped &out, geometry_msgs::WrenchStamped &bias)
00188 {
00189   out.header.stamp = in.header.stamp;
00190   out.header.frame_id = in.header.frame_id;
00191   out.wrench.force.x = in.wrench.force.x - bias.wrench.force.x;
00192   out.wrench.force.y = in.wrench.force.y - bias.wrench.force.y;
00193   out.wrench.force.z = in.wrench.force.z - bias.wrench.force.z;
00194   out.wrench.torque.x = in.wrench.torque.x - bias.wrench.torque.x;
00195   out.wrench.torque.y = in.wrench.torque.y - bias.wrench.torque.y;
00196   out.wrench.torque.z = in.wrench.torque.z - bias.wrench.torque.z;
00197 }
00198 
00199 void NetftUtils::applyThreshold(double &value, double thresh)
00200 {
00201   if(value <= thresh && value >= -thresh)
00202   {
00203     value = 0.0;
00204   }
00205 }
00206 
00207 void NetftUtils::transformFrame(geometry_msgs::WrenchStamped in_data, geometry_msgs::WrenchStamped &out_data, char target_frame)
00208 {
00209   tf::Vector3 tempF;
00210   tf::Vector3 tempT;
00211   tempF.setX(in_data.wrench.force.x);
00212   tempF.setY(in_data.wrench.force.y);
00213   tempF.setZ(in_data.wrench.force.z);
00214   tempT.setX(in_data.wrench.torque.x);
00215   tempT.setY(in_data.wrench.torque.y);
00216   tempT.setZ(in_data.wrench.torque.z);
00217   if(target_frame == 'w')
00218   {
00219       out_data.header.frame_id = world_frame;
00220       tempF = ft_to_world * tempF;
00221       tempT = ft_to_world * tempT;
00222   }
00223   else if(target_frame == 't')
00224   {
00225       out_data.header.frame_id = ft_frame;
00226       tempF = ft_to_world.inverse() * tempF;
00227       tempT = ft_to_world.inverse() * tempT;
00228   }     
00229   out_data.header.stamp = in_data.header.stamp;
00230   out_data.wrench.force.x = tempF.getX();
00231   out_data.wrench.force.y = tempF.getY();
00232   out_data.wrench.force.z = tempF.getZ();
00233   out_data.wrench.torque.x = tempT.getX();
00234   out_data.wrench.torque.y = tempT.getY();
00235   out_data.wrench.torque.z = tempT.getZ();
00236 }
00237 
00238 // Runs when a new datapoint comes in
00239 void NetftUtils::netftCallback(const geometry_msgs::WrenchStamped::ConstPtr& data)
00240 {
00241   // Filter data
00242   std::vector<double> tempData;
00243   tempData.resize(6);
00244   tempData.at(0) = -data->wrench.force.x;
00245   tempData.at(1) = data->wrench.force.y;
00246   tempData.at(2) = data->wrench.force.z;
00247   tempData.at(3) = -data->wrench.torque.x;
00248   tempData.at(4) = data->wrench.torque.y;
00249   tempData.at(5) = data->wrench.torque.z;
00250   
00251   if(isFilterOn && !newFilter)
00252     lp->update(tempData,tempData);
00253   
00254   // Copy tool frame data. apply negative to x data to follow right hand rule convention (ft raw data does not)
00255   raw_data_tool.header.stamp = data->header.stamp;
00256   raw_data_tool.header.frame_id = ft_frame;
00257   raw_data_tool.wrench.force.x = tempData.at(0);
00258   raw_data_tool.wrench.force.y = tempData.at(1);
00259   raw_data_tool.wrench.force.z = tempData.at(2);
00260   raw_data_tool.wrench.torque.x = tempData.at(3);
00261   raw_data_tool.wrench.torque.y = tempData.at(4);
00262   raw_data_tool.wrench.torque.z = tempData.at(5);
00263   
00264   // Copy in new netft data in tool frame and transform to world frame
00265   transformFrame(raw_data_tool, raw_data_world, 'w');
00266   
00267   if (isGravityBiased) // Compensate for gravity. Assumes world Z-axis is up
00268   {
00269       // Gravity moment = (payloadLeverArm) cross (payload force)  <== all in the sensor frame. Need to convert to world later
00270       // Since it's assumed that the CoM of the payload is on the sensor's central axis, this calculation is simplified.
00271       double gravMomentX = -payloadLeverArm*tf_data_tool.wrench.force.y;
00272       double gravMomentY = payloadLeverArm*tf_data_tool.wrench.force.x;
00273     
00274       // Subtract the gravity torques from the previously-calculated wrench in the tool frame
00275       tf_data_tool.wrench.torque.x = tf_data_tool.wrench.torque.x - gravMomentX;
00276       tf_data_tool.wrench.torque.y = tf_data_tool.wrench.torque.y - gravMomentY;
00277     
00278       // Convert to world to account for the gravity force. Assumes world-Z is up.
00279       //ROS_INFO_STREAM("gravity force in the world Z axis: "<< payloadWeight);
00280       transformFrame(tf_data_tool, tf_data_world, 'w');
00281       tf_data_world.wrench.force.z = tf_data_world.wrench.force.z - payloadWeight;
00282     
00283       // tf_data_world now accounts for gravity completely. Convert back to the tool frame to make that data available, too
00284       transformFrame(tf_data_world, tf_data_tool, 't');
00285   }
00286   
00287   if (isBiased) // Apply the bias for a static sensor frame
00288   {
00289     // Get tool bias in world frame
00290     geometry_msgs::WrenchStamped world_bias;
00291     transformFrame(bias, world_bias, 'w');
00292     // Add bias and apply threshold to get transformed data
00293     copyWrench(raw_data_world, tf_data_world, world_bias);
00294   }
00295   else // Just pass the data straight through
00296   {
00297     copyWrench(raw_data_world, tf_data_world, zero_wrench);
00298     copyWrench(raw_data_tool, tf_data_tool, zero_wrench);
00299   }
00300                   
00301   // Apply thresholds
00302   applyThreshold(tf_data_world.wrench.force.x, threshold.wrench.force.x);
00303   applyThreshold(tf_data_world.wrench.force.y, threshold.wrench.force.y);
00304   applyThreshold(tf_data_world.wrench.force.z, threshold.wrench.force.z);
00305   applyThreshold(tf_data_world.wrench.torque.x, threshold.wrench.torque.x);
00306   applyThreshold(tf_data_world.wrench.torque.y, threshold.wrench.torque.y);
00307   applyThreshold(tf_data_world.wrench.torque.z, threshold.wrench.torque.z);
00308   applyThreshold(tf_data_tool.wrench.force.x, threshold.wrench.force.x);
00309   applyThreshold(tf_data_tool.wrench.force.y, threshold.wrench.force.y);
00310   applyThreshold(tf_data_tool.wrench.force.z, threshold.wrench.force.z);
00311   applyThreshold(tf_data_tool.wrench.torque.x, threshold.wrench.torque.x);
00312   applyThreshold(tf_data_tool.wrench.torque.y, threshold.wrench.torque.y);
00313   applyThreshold(tf_data_tool.wrench.torque.z, threshold.wrench.torque.z);
00314   //ROS_INFO_STREAM("Callback time: " << tf_data_tool.header.stamp.toSec()-ros::Time::now().toSec());
00315 }                 
00316 
00317 // Set the readings from the sensor to zero at this instant and continue to apply the bias on future readings.
00318 // This doesn't account for gravity.
00319 // Useful when the sensor's orientation won't change.
00320 // Run this method when the sensor is stationary to avoid inertial effects.
00321 // Cannot bias the sensor if gravity compensation has already been applied.
00322 bool NetftUtils::fixedOrientationBias(netft_utils::SetBias::Request &req, netft_utils::SetBias::Response &res)
00323 {                 
00324   if(req.toBias)  
00325   {           
00326     copyWrench(raw_data_tool, bias, zero_wrench); // Store the current wrench readings in the 'bias' variable, to be applied hereafter
00327     if(req.forceMax >= 0.0001) // if forceMax was specified and > 0
00328       forceMaxB = req.forceMax;
00329     if(req.torqueMax >= 0.0001)
00330       torqueMaxB = req.torqueMax; // if torqueMax was specified and > 0
00331     
00332     isNewBias = true;
00333     isBiased = true;
00334   }               
00335   else            
00336   {               
00337     copyWrench(zero_wrench, bias, zero_wrench); // Clear the stored bias if the argument was false
00338   }               
00339 
00340   res.success = true;
00341                   
00342   return true;    
00343 }
00344 
00345 // Calculate the payload's mass and center of mass so gravity can be compensated for, even as the sensor changes orientation.
00346 // It's assumed that the payload's center of mass is located on the sensor's central access.
00347 // Run this method when the sensor is stationary to avoid inertial effects.
00348 // Cannot do gravity compensation if sensor has already been biased.
00349 bool NetftUtils::compensateForGravity(netft_utils::SetBias::Request &req, netft_utils::SetBias::Response &res)
00350 {
00351 
00352   if(req.toBias)
00353   {  
00354     if (isBiased)
00355     {
00356       ROS_ERROR("Cannot compensate for gravity if the sensor has already been biased, i.e. useful data was wiped out");
00357       res.success = false;
00358       return false;  
00359     }
00360     else  // Cannot compensate for gravity if the sensor has already been biased, i.e. useful data was wiped out 
00361     {
00362       // Get the weight of the payload. Assumes the world Z axis is up.
00363       payloadWeight = raw_data_world.wrench.force.z;
00364   
00365       // Calculate the z-coordinate of the payload's center of mass, in the sensor frame.
00366       // It's assumed that the x- and y-coordinates are zero.
00367       // This is a lever arm.
00368       payloadLeverArm = raw_data_tool.wrench.torque.y/raw_data_tool.wrench.force.x;
00369     
00370       isNewGravityBias = true;
00371       isGravityBiased = true;
00372     }
00373   }
00374   
00375   res.success = true;     
00376   return true;
00377 }  
00378 
00379 bool NetftUtils::setFilter(netft_utils::SetFilter::Request &req, netft_utils::SetFilter::Response &res)
00380 {                 
00381   if(req.toFilter)  
00382   {
00383     newFilter = true;
00384     isFilterOn = true;
00385     deltaTFilter = req.deltaT;
00386     cutoffFrequency = req.cutoffFrequency;
00387   }               
00388   else            
00389   {               
00390     isFilterOn = false;
00391   }               
00392   
00393   return true;    
00394 }  
00395 
00396 bool NetftUtils::setMax(netft_utils::SetMax::Request &req, netft_utils::SetMax::Response &res)
00397 {                 
00398   if(req.forceMax >= 0.0001)
00399     forceMaxU = req.forceMax;
00400   if(req.torqueMax >= 0.0001)
00401     torqueMaxU = req.torqueMax;
00402   
00403   res.success = true;             
00404   return true;    
00405 }
00406 
00407 bool NetftUtils::setWeightBias(netft_utils::SetBias::Request &req, netft_utils::SetBias::Response &res)
00408 {                 
00409   if(req.toBias)  
00410   {               
00411     copyWrench(raw_data_tool, weight_bias, zero_wrench);
00412   }               
00413   else            
00414   {               
00415     copyWrench(zero_wrench, weight_bias, zero_wrench);
00416   }               
00417   res.success = true;
00418                   
00419   return true;    
00420 }  
00421     
00422 bool NetftUtils::getWeight(netft_utils::GetDouble::Request &req, netft_utils::GetDouble::Response &res)
00423 {                 
00424   geometry_msgs::WrenchStamped carried_weight;
00425   copyWrench(raw_data_tool, carried_weight, weight_bias);
00426   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;
00427                   
00428   return true;    
00429 }      
00430             
00431 bool NetftUtils::setThreshold(netft_utils::SetThreshold::Request &req, netft_utils::SetThreshold::Response &res)
00432 {                 
00433   threshold.wrench.force.x = req.data.wrench.force.x;
00434   threshold.wrench.force.y = req.data.wrench.force.y;
00435   threshold.wrench.force.z = req.data.wrench.force.z;
00436   threshold.wrench.torque.x = req.data.wrench.torque.x;
00437   threshold.wrench.torque.y = req.data.wrench.torque.y;
00438   threshold.wrench.torque.z = req.data.wrench.torque.z;
00439                   
00440   res.success = true;
00441                   
00442   return true;    
00443 }
00444 
00445 void NetftUtils::checkMaxForce()
00446 {
00447   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);
00448   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);
00449   double fMax;
00450   double tMax;
00451   if(isBiased && !isNewBias)
00452   {
00453     fMax = forceMaxB;
00454     tMax = torqueMaxB;
00455   }
00456   else
00457   {
00458     if(isBiased && isNewBias)
00459     {
00460       isNewBias = false;
00461     }
00462 
00463     fMax = forceMaxU; //50.0;
00464     tMax = torqueMaxU; //5.0;
00465   }
00466 
00467   // If max FT exceeded, send cancel unless we have just sent it MAX_CANCEL times
00468   //ROS_INFO("FMAG: %f TMAG: %f", fMag, tMag);
00469   if((fabs(fMag) > fMax || fabs(tMag) > tMax) && cancel_count > 0)
00470   {
00471     cancel_msg.toCancel = true;
00472     //ROS_INFO("Force torque violation. Canceling move.");
00473     ROS_INFO("FMAG: %f FMAX: %f TMAG:%f TMAX: %f count: %d wait: %d", fMag, fMax, tMag, tMax, cancel_count, cancel_wait);
00474     cancel_count-=1;
00475   }
00476   // If we have sent cancel MAX_CANCEL times, don't send cancel again for MAX_WAIT cycles
00477   else if(cancel_count == 0 && cancel_wait > 0 && cancel_wait <= MAX_WAIT)
00478   {
00479     cancel_msg.toCancel = false;
00480     cancel_wait-=1;
00481   }
00482   // If we have just finished waiting MAX_WAIT times, or the max force is no longer exceeded, reset cancel_count and cancel_wait
00483   else if(cancel_wait == 0 || !(fabs(fMag) > fMax || fabs(tMag) > tMax))
00484   {
00485     cancel_msg.toCancel = false;
00486     cancel_count = MAX_CANCEL;
00487     cancel_wait = MAX_WAIT;
00488   }
00489 }               
00490                   


netft_utils
Author(s): Alex von Sternberg , Derek King, Andy Zelenak
autogenerated on Sat Jul 15 2017 02:45:58