cob_forcetorque: forcetorque.cpp Source File

00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_driver
00012  * ROS package name: cob_forcetorque
00013  *                                                              
00014  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00015  *                      
00016  * Author: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de
00017  * Supervised by: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de
00018  *
00019  * Date of creation: June 2010
00020  * ToDo:
00021  *
00022  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00023  *
00024  * Redistribution and use in source and binary forms, with or without
00025  * modification, are permitted provided that the following conditions are met:
00026  *
00027  *     * Redistributions of source code must retain the above copyright
00028  *       notice, this list of conditions and the following disclaimer.
00029  *     * Redistributions in binary form must reproduce the above copyright
00030  *       notice, this list of conditions and the following disclaimer in the
00031  *       documentation and/or other materials provided with the distribution.
00032  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00033  *       Engineering and Automation (IPA) nor the names of its
00034  *       contributors may be used to endorse or promote products derived from
00035  *       this software without specific prior written permission.
00036  *
00037  * This program is free software: you can redistribute it and/or modify
00038  * it under the terms of the GNU Lesser General Public License LGPL as 
00039  * published by the Free Software Foundation, either version 3 of the 
00040  * License, or (at your option) any later version.
00041  * 
00042  * This program is distributed in the hope that it will be useful,
00043  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00044  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00045  * GNU Lesser General Public License LGPL for more details.
00046  * 
00047  * You should have received a copy of the GNU Lesser General Public 
00048  * License LGPL along with this program. 
00049  * If not, see <http://www.gnu.org/licenses/>.
00050  *
00051  ****************************************************************/
00052 
00053 
00054 #include <stdint.h>
00055 typedef unsigned char uint8_t;
00056 #include <inttypes.h>
00057 #include <iostream>
00058 #include <ros/ros.h>
00059 #include <std_msgs/Float32MultiArray.h>
00060 #include <cob_forcetorque/ForceTorqueCtrl.h>
00061 
00062 #include <cob_srvs/Trigger.h>
00063 
00064 #include <tf/transform_listener.h>
00065 #include <visualization_msgs/Marker.h>
00066 
00067 #include <math.h>
00068 #include <iostream>
00069 #define PI 3.14159265
00070 
00071 
00072 class ForceTorqueNode
00073 {
00074 public:
00075   // create a handle for this node, initialize node
00076   ros::NodeHandle nh_;
00077 
00078   bool init();
00079   bool srvCallback_Init(cob_srvs::Trigger::Request &req,
00080                         cob_srvs::Trigger::Response &res );
00081   bool srvCallback_Calibrate(cob_srvs::Trigger::Request &req,
00082                         cob_srvs::Trigger::Response &res );
00083   void updateFTData();
00084   void visualizeData(double x, double y, double z);
00085 
00086 private:
00087   // declaration of topics to publish
00088   ros::Publisher topicPub_ForceData_;
00089   ros::Publisher topicPub_ForceDataBase_;
00090   ros::Publisher topicPub_Marker_;
00091 
00092   // service servers
00093   ros::ServiceServer srvServer_Init_;
00094   ros::ServiceServer srvServer_Calibrate_;
00095 
00096   tf::TransformListener tflistener;
00097   tf::StampedTransform transform_ee_base;
00098 
00099   bool m_isInitialized;
00100   ForceTorqueCtrl ftc;
00101   std::vector<double> F_avg;
00102   
00103 };
00104 
00105 bool ForceTorqueNode::init()
00106 {
00107   m_isInitialized = false;
00108   topicPub_ForceData_ = nh_.advertise<std_msgs::Float32MultiArray>("force_values", 100);
00109   topicPub_ForceDataBase_ = nh_.advertise<std_msgs::Float32MultiArray>("force_values_base", 100);
00110   topicPub_Marker_ = nh_.advertise<visualization_msgs::Marker>("/visualization_marker", 1);
00111   srvServer_Init_ = nh_.advertiseService("Init", &ForceTorqueNode::srvCallback_Init, this);
00112   srvServer_Calibrate_ = nh_.advertiseService("Calibrate", &ForceTorqueNode::srvCallback_Calibrate, this);
00113 
00114   
00115   return true;
00116 }
00117 
00118 bool ForceTorqueNode::srvCallback_Init(cob_srvs::Trigger::Request &req,
00119                       cob_srvs::Trigger::Response &res )
00120 {
00121   if(!m_isInitialized)
00122     {
00123         ftc.SetFXGain(-1674.08485641479, 25.3936432491561, 3936.02718786968, -26695.2539299392, -3463.73728677908, 32320.8777656041);
00124         ftc.SetFYGain(-4941.11252317989, 32269.5827812235, 1073.82949467087, -15541.8400780814, 3061.89541712948, -18995.9891819409);
00125         ftc.SetFZGain(39553.9250733854, -501.940034213822, 40905.2545309848, 85.1095865539103, 38879.4015426067, 541.344775537753);
00126         ftc.SetTXGain(-57.4775857386444, 225.941430274037, -638.238694389357, -116.780649376712, 645.133934885308, -116.310081348745 );
00127         ftc.SetTYGain(786.70602313107, -4.36504382717595, -422.360387149734, 180.7428885668, -352.389412256677, -232.293941041101);
00128         ftc.SetTZGain(60.1009854270179, -400.19573754971, 29.142908672741, -392.119024237625, 70.9306507180567, -478.104759057292);
00129 
00130         ftc.SetCalibMatrix();
00131         ftc.Init();
00132         ROS_INFO("FTC initialized");
00133 
00134         //set Calibdata to zero
00135         F_avg.resize(6);
00136         F_avg[0] = 0.0;
00137         F_avg[1] = 0.0;
00138         F_avg[2] = 0.0;
00139         F_avg[3] = 0.0;
00140         F_avg[4] = 0.0;
00141         F_avg[5] = 0.0;
00142 
00143 
00144         m_isInitialized = true;
00145     }
00146   return true;
00147 }
00148 
00149 bool ForceTorqueNode::srvCallback_Calibrate(cob_srvs::Trigger::Request &req,
00150                       cob_srvs::Trigger::Response &res )
00151 {
00152   int measurements = 20;
00153   if(m_isInitialized)
00154     {
00155       F_avg[0] = 0.0;
00156       F_avg[1] = 0.0;
00157       F_avg[2] = 0.0;
00158       F_avg[3] = 0.0;
00159       F_avg[4] = 0.0;
00160       F_avg[5] = 0.0;
00161       for(int i = 0; i < measurements; i++)
00162         {
00163           double Fx, Fy, Fz, Tx, Ty, Tz = 0;
00164           ftc.ReadSGData(Fx, Fy, Fz, Tx, Ty, Tz);
00165           F_avg[0] += Fx;
00166           F_avg[1] += Fy;
00167           F_avg[2] += Fz;
00168           F_avg[3] += Tx;
00169           F_avg[4] += Ty;
00170           F_avg[5] += Tz;
00171           usleep(10000);
00172         }
00173       for(int i = 0; i < 6; i++)
00174         F_avg[i] /= measurements;
00175       return true;
00176     }
00177   else
00178     return false;
00179 }
00180 
00181 void ForceTorqueNode::updateFTData()
00182 {
00183   if(m_isInitialized)
00184     {
00185       double Fx, Fy, Fz, Tx, Ty, Tz = 0;
00186       
00187       ftc.ReadSGData(Fx, Fy, Fz, Tx, Ty, Tz);
00188       std_msgs::Float32MultiArray msg;
00189       msg.data.push_back(Fx-F_avg[0]);
00190       msg.data.push_back(Fy-F_avg[1]);
00191       msg.data.push_back(Fz-F_avg[2]);
00192       msg.data.push_back(Tx-F_avg[3]);
00193       msg.data.push_back(Ty-F_avg[4]);
00194       msg.data.push_back(Tz-F_avg[5]);
00195 
00196 
00197       topicPub_ForceData_.publish(msg);
00198       
00199 
00200       tf::Transform fdata_base;
00201       tf::Transform fdata;
00202       fdata.setOrigin(tf::Vector3(Fx-F_avg[0], Fy-F_avg[1], Fz-F_avg[2]));
00203 
00204       try{
00205         tflistener.lookupTransform("arm_7_link", "base_link", ros::Time(0), transform_ee_base);
00206       }
00207       catch (tf::TransformException ex){
00208         ROS_ERROR("%s",ex.what());
00209       }
00210 
00211       fdata_base = transform_ee_base * fdata;
00212 
00213       std_msgs::Float32MultiArray base_msg;
00214       base_msg.data.push_back(fdata_base.getOrigin().x());
00215       base_msg.data.push_back(fdata_base.getOrigin().y());
00216       base_msg.data.push_back(fdata_base.getOrigin().z());
00217       base_msg.data.push_back(0.0);
00218       base_msg.data.push_back(0.0);
00219       base_msg.data.push_back(0.0);
00220       topicPub_ForceDataBase_.publish(base_msg);
00221       visualizeData(fdata_base.getOrigin().x(), fdata_base.getOrigin().y(), fdata_base.getOrigin().z());
00222     }
00223 }
00224 
00225 void ForceTorqueNode::visualizeData(double x, double y, double z)
00226 {
00227   visualization_msgs::Marker marker;
00228   uint32_t shape = visualization_msgs::Marker::ARROW;
00229   marker.header.frame_id = "/arm_7_link";
00230   marker.header.stamp = ros::Time::now();
00231   marker.ns = "ForceTorqueData";
00232   marker.id = 0;
00233   marker.type = shape;
00234 // first delete old markers
00235   marker.action = visualization_msgs::Marker::DELETE;
00236   topicPub_Marker_.publish(marker);     
00237   marker.action = visualization_msgs::Marker::ADD;
00238   marker.pose.position.x = 0;
00239   marker.pose.position.y = 0;
00240   marker.pose.position.z = 0;
00241   marker.scale.x = x/100;
00242   marker.scale.y = y/100;
00243   marker.scale.z = z/100;
00244   marker.color.r = 0.0f;
00245   marker.color.g = 1.0f;
00246   marker.color.b = 0.0f;
00247   marker.color.a = 1.0;
00248   marker.lifetime = ros::Duration();
00249   topicPub_Marker_.publish(marker);
00250 }
00251 
00252 
00253 
00254 
00255 
00256 
00257 int main(int argc, char ** argv)
00258 {
00259 
00260   ros::init(argc, argv, "talker");
00261   ForceTorqueNode ftn;
00262   ftn.init();
00263   
00264   ROS_INFO("ForceTorque Sensor Node running.");
00265   
00266   ros::Rate loop_rate(10);
00267   while(ros::ok())
00268     {
00269       ros::spinOnce();
00270       loop_rate.sleep();
00271       ftn.updateFTData();
00272     }
00273   return 0;
00274 }
00275