00001 /* 00002 * ft_calib.h 00003 * 00004 * Least squares calibration of: 00005 * - Bias of F/T sensor 00006 * - Mass of attached gripper 00007 * - Location of the center of mass of the gripper 00008 * 00009 * Requires calibrated accelerometer readings 00010 * (calibrated with respect to the robot). 00011 * 00012 * 00013 * Created on: Sep 26, 2012 00014 * Authors: Francisco Viña 00015 * fevb <at> kth.se 00016 */ 00017 00018 /* Copyright (c) 2012, Francisco Viña, CVAP, KTH 00019 All rights reserved. 00020 00021 Redistribution and use in source and binary forms, with or without 00022 modification, are permitted provided that the following conditions are met: 00023 * Redistributions of source code must retain the above copyright 00024 notice, this list of conditions and the following disclaimer. 00025 * Redistributions in binary form must reproduce the above copyright 00026 notice, this list of conditions and the following disclaimer in the 00027 documentation and/or other materials provided with the distribution. 00028 * Neither the name of KTH nor the 00029 names of its contributors may be used to endorse or promote products 00030 derived from this software without specific prior written permission. 00031 00032 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00033 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00034 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00035 DISCLAIMED. IN NO EVENT SHALL KTH BE LIABLE FOR ANY 00036 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00037 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00038 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00039 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00040 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00041 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00042 */ 00043 00044 00045 00046 #ifndef FTCALIB_H_ 00047 #define FTCALIB_H_ 00048 #include <geometry_msgs/Vector3Stamped.h> 00049 #include <geometry_msgs/WrenchStamped.h> 00050 #include <eigen3/Eigen/Core> 00051 00052 00053 // Least Squares calibration of bias of FT sensor and the mass and location of the COM of the gripper 00054 namespace Calibration{ 00055 class FTCalib 00056 { 00057 public: 00058 00059 FTCalib(); 00060 virtual ~FTCalib(); 00061 00062 00063 // adds a F/T measurement and the corresponding measurement matrix from the gravity 00064 // measurements of the accelerometer 00065 // gravity is assumed to be expressed in the F/T sensor frame 00066 virtual void addMeasurement(const geometry_msgs::Vector3Stamped &gravity, 00067 const geometry_msgs::WrenchStamped &ft_raw); 00068 00069 00070 // Least squares to estimate the F/T sensor parameters 00071 // The estimated parameters are : 00072 // [m m*cx m*cy m*cz FBx FBy FBz TBx TBy TBz] 00073 // m: mass of the gripper 00074 // [cx, cy, cz] are the coordinates of the center of mass of the gripper 00075 // FB : force bias 00076 // TB: torque bias 00077 // All expressed in the FT sensor frame 00078 virtual Eigen::VectorXd getCalib(); 00079 00080 00081 00082 protected: 00083 00084 Eigen::MatrixXd H; // stacked measurement matrices 00085 Eigen::VectorXd Z; // stacked F/T measurements 00086 // FT_sensor_frame_acc taken as 0 00087 00088 unsigned int m_num_meas; // number of stacked measurements; 00089 00090 // measurement matrix based on "On-line Rigid Object Recognition and Pose Estimation 00091 // Based on Inertial Parameters", D. Kubus, T. Kroger, F. Wahl, IROS 2008 00092 virtual Eigen::MatrixXd getMeasurementMatrix(const geometry_msgs::Vector3Stamped &gravity); 00093 00094 }; 00095 } 00096 00097 00098 #endif /* INERTIALPARAMESTIMATOR_H_ */