gravity_compensation.cpp
Go to the documentation of this file.
00001 /*
00002  *  gravity_compensation.cpp
00003  *
00004  *  Created on: Nov 11, 2013
00005  *  Authors:   Francisco Viña
00006  *            fevb <at> kth.se
00007  */
00008 
00009 /* Copyright (c) 2013, Francisco Viña, CVAP, KTH
00010    All rights reserved.
00011 
00012    Redistribution and use in source and binary forms, with or without
00013    modification, are permitted provided that the following conditions are met:
00014       * Redistributions of source code must retain the above copyright
00015         notice, this list of conditions and the following disclaimer.
00016       * Redistributions in binary form must reproduce the above copyright
00017         notice, this list of conditions and the following disclaimer in the
00018         documentation and/or other materials provided with the distribution.
00019       * Neither the name of KTH nor the
00020         names of its contributors may be used to endorse or promote products
00021         derived from this software without specific prior written permission.
00022 
00023    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00024    ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00025    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00026    DISCLAIMED. IN NO EVENT SHALL KTH BE LIABLE FOR ANY
00027    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00028    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00030    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00031    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00032    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00033 */
00034 
00035 #include <ros/ros.h>
00036 #include <gravity_compensation/gravity_compensation.h>
00037 #include <geometry_msgs/PoseStamped.h>
00038 #include <tf_conversions/tf_eigen.h>
00039 #include <eigen3/Eigen/Core>
00040 #include <eigen_conversions/eigen_msg.h>
00041 
00042 
00043 GravityCompensation::GravityCompensation(GravityCompensationParams *g_comp_params)
00044 {
00045     m_g_comp_params = g_comp_params;
00046     m_tf_listener = new tf::TransformListener();
00047 }
00048 
00049 GravityCompensation::~GravityCompensation()
00050 {
00051     delete m_tf_listener;
00052 }
00053 
00054 
00055 void GravityCompensation::Zero(const geometry_msgs::WrenchStamped &ft_raw,
00056                           geometry_msgs::WrenchStamped &ft_zeroed)
00057 {
00058     Eigen::Matrix<double, 6, 1> bias = m_g_comp_params->getBias();
00059 
00060     ft_zeroed = ft_raw;
00061     Eigen::Matrix<double, 6, 1> ft_raw_eigen;
00062     Eigen::Matrix<double, 6, 1> ft_zeroed_eigen;
00063     tf::wrenchMsgToEigen(ft_raw.wrench, ft_raw_eigen);
00064 
00065     ft_zeroed_eigen = ft_raw_eigen - bias;
00066     tf::wrenchEigenToMsg(ft_zeroed_eigen, ft_zeroed.wrench);
00067 }
00068 
00069 
00070 bool GravityCompensation::Compensate(const geometry_msgs::WrenchStamped &ft_zeroed,
00071                                      const sensor_msgs::Imu &gravity,
00072                                      geometry_msgs::WrenchStamped &ft_compensated)
00073 {
00074 
00075     geometry_msgs::Vector3Stamped g;
00076     g.vector = gravity.linear_acceleration;
00077     g.header = gravity.header;
00078     g.header.stamp = ros::Time();
00079 
00080     // convert the accelerometer reading to the F/T sensor frame
00081     geometry_msgs::Vector3Stamped g_ft_frame;
00082     try
00083     {
00084         m_tf_listener->transformVector(ft_zeroed.header.frame_id, g, g_ft_frame);
00085     }
00086 
00087     catch(tf::TransformException &ex)
00088     {
00089         ROS_ERROR("Error transforming gravity vector to ft sensor frame...");
00090         ROS_ERROR("%s.", ex.what());
00091         return false;
00092     }
00093 
00094 
00095     double gripper_mass = m_g_comp_params->getGripperMass();
00096     Eigen::Vector3d g_ft_frame_eigen;
00097     tf::vectorMsgToEigen(g_ft_frame.vector, g_ft_frame_eigen);
00098 
00099 
00100     Eigen::Matrix<double, 6, 1> gripper_wrench_eigen;
00101     gripper_wrench_eigen.topRows(3) = g_ft_frame_eigen*gripper_mass;
00102 
00103 
00104     // grab the gripper_com
00105     tf::StampedTransform gripper_com_tf = m_g_comp_params->getGripperCOM();
00106 
00107     // convert the gripper_com to geometry_msgs::PoseStamped
00108     geometry_msgs::PoseStamped gripper_com;
00109     tf::poseTFToMsg(gripper_com_tf, gripper_com.pose);
00110     gripper_com.header.stamp = ros::Time();
00111     gripper_com.header.frame_id = gripper_com_tf.frame_id_;
00112 
00113     // make sure the gripper COM is expressed with respect to the F/T sensor frame
00114     geometry_msgs::PoseStamped ft_gripper_com;
00115 
00116     try
00117     {
00118         m_tf_listener->transformPose(ft_zeroed.header.frame_id,
00119                                      gripper_com,
00120                                      ft_gripper_com);
00121     }
00122 
00123     catch(tf::TransformException &ex)
00124     {
00125         ROS_ERROR("Error looking up transform between the gripper COM and the ft sensor frame");
00126         ROS_ERROR("%s.", ex.what());
00127         return false;
00128     }
00129 
00130     // convert to Eigen to do cross product to compute the torque
00131     Eigen::Vector3d r;
00132     tf::pointMsgToEigen(ft_gripper_com.pose.position, r);
00133 
00134     // compute torque generated by weight of the gripper
00135     Eigen::Vector3d gripper_force_eigen = gripper_wrench_eigen.topRows(3);
00136     Eigen::Vector3d gripper_torque_eigen = r.cross(gripper_force_eigen);
00137     gripper_wrench_eigen.bottomRows(3) = gripper_torque_eigen;
00138 
00139     // compensate force & torque
00140     Eigen::Matrix<double, 6, 1> ft_zeroed_eigen;
00141     tf::wrenchMsgToEigen(ft_zeroed.wrench, ft_zeroed_eigen);
00142     Eigen::Matrix<double, 6, 1> ft_compensated_eigen;
00143 
00144     ft_compensated_eigen = ft_zeroed_eigen - gripper_wrench_eigen;
00145 
00146     tf::wrenchEigenToMsg(ft_compensated_eigen, ft_compensated.wrench);
00147     ft_compensated.header = ft_zeroed.header;
00148 
00149     return true;
00150 }


gravity_compensation
Author(s): Francisco Vina
autogenerated on Sun Oct 5 2014 23:59:50