ft_calib.h
Go to the documentation of this file.
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_ */


force_torque_sensor_calib
Author(s): Francisco Vina
autogenerated on Sat Jun 8 2019 18:27:53