Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <ros/ros.h>
00037 #include <force_torque_sensor_calib/ft_calib.h>
00038 #include <eigen3/Eigen/Dense>
00039 #include <kdl/frameacc.hpp>
00040 #include <kdl/frames.hpp>
00041
00042 namespace Calibration{
00043
00044 FTCalib::FTCalib()
00045 {
00046 m_num_meas = 0;
00047 }
00048
00049 FTCalib::~FTCalib(){
00050
00051 }
00052
00053 void FTCalib::addMeasurement(const geometry_msgs::Vector3Stamped &gravity,
00054 const geometry_msgs::WrenchStamped &ft_raw)
00055 {
00056 if(gravity.header.frame_id != ft_raw.header.frame_id)
00057 {
00058 ROS_ERROR("Gravity vector and ft raw expressed in different frames (%s, %s)!",
00059 gravity.header.frame_id.c_str(), ft_raw.header.frame_id.c_str());
00060 return;
00061 }
00062
00063 m_num_meas++;
00064
00065 Eigen::MatrixXd h = getMeasurementMatrix(gravity);
00066 Eigen::VectorXd z = Eigen::Matrix<double, 6, 1>::Zero();
00067 z(0) = ft_raw.wrench.force.x;
00068 z(1) = ft_raw.wrench.force.y;
00069 z(2) = ft_raw.wrench.force.z;
00070
00071 z(3) = ft_raw.wrench.torque.x;
00072 z(4) = ft_raw.wrench.torque.y;
00073 z(5) = ft_raw.wrench.torque.z;
00074
00075
00076
00077 if(m_num_meas==1)
00078 {
00079 H = h;
00080 Z = z;
00081 }
00082
00083 else
00084 {
00085 Eigen::MatrixXd H_temp = H;
00086 Eigen::VectorXd Z_temp = Z;
00087
00088 H.resize(m_num_meas*6, 10);
00089 Z.resize(m_num_meas*6);
00090
00091 H.topRows((m_num_meas-1)*6) = H_temp;
00092 Z.topRows((m_num_meas-1)*6) = Z_temp;
00093
00094 H.bottomRows(6) = h;
00095 Z.bottomRows(6) = z;
00096 }
00097
00098
00099 }
00100
00101
00102
00103 Eigen::VectorXd FTCalib::getCalib()
00104 {
00105 Eigen::VectorXd ft_calib_params(10);
00106
00107 ft_calib_params = H.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Z);
00108
00109 return ft_calib_params;
00110 }
00111
00112
00113 Eigen::MatrixXd FTCalib::getMeasurementMatrix(const geometry_msgs::Vector3Stamped &gravity)
00114 {
00115 KDL::Vector w = KDL::Vector::Zero();
00116 KDL::Vector alpha = KDL::Vector::Zero();
00117 KDL::Vector a = KDL::Vector::Zero();
00118
00119 KDL::Vector g(gravity.vector.x, gravity.vector.y, gravity.vector.z);
00120
00121 Eigen::MatrixXd H;
00122 H = Eigen::Matrix<double, 6, 10>::Zero();
00123
00124 for(unsigned int i=0; i<3; i++)
00125 {
00126 for(unsigned int j=4; j<10; j++)
00127 {
00128 if(i==j-4)
00129 {
00130 H(i,j) = 1.0;
00131 }
00132 else
00133 {
00134 H(i,j) = 0.0;
00135 }
00136 }
00137 }
00138
00139 for(unsigned int i=3; i<6; i++)
00140 {
00141 H(i,0) = 0.0;
00142 }
00143
00144 H(3,1) = 0.0;
00145 H(4,2) = 0.0;
00146 H(5,3) = 0.0;
00147
00148 for(unsigned int i=0; i<3; i++)
00149 {
00150 H(i,0) = a(i) - g(i);
00151 }
00152
00153 H(0,1) = -w(1)*w(1) - w(2)*w(2);
00154 H(0,2) = w(0)*w(1) - alpha(2);
00155 H(0,3) = w(0)*w(2) + alpha(1);
00156
00157 H(1,1) = w(0)*w(1) + alpha(2);
00158 H(1,2) = -w(0)*w(0) - w(2)*w(2);
00159 H(1,3) = w(1)*w(2) - alpha(0);
00160
00161 H(2,1) = w(0)*w(2) - alpha(1);
00162 H(2,2) = w(1)*w(2) + alpha(0);
00163 H(2,3) = -w(1)*w(1) - w(0)*w(0);
00164
00165 H(3,2) = a(2) - g(2);
00166 H(3,3) = -a(1) + g(1);
00167
00168
00169 H(4,1) = -a(2) + g(2);
00170 H(4,3) = a(0) - g(0);
00171
00172 H(5,1) = a(1) - g(1);
00173 H(5,2) = -a(0) + g(0);
00174
00175 for(unsigned int i=3; i<6; i++)
00176 {
00177 for(unsigned int j=4; j<10; j++)
00178 {
00179 if(i==(j-4))
00180 {
00181 H(i,j) = 1.0;
00182 }
00183 else
00184 {
00185 H(i,j) = 0.0;
00186 }
00187 }
00188 }
00189
00190
00191 return H;
00192 }
00193
00194 }