38 #include <Eigen/Dense> 39 #include <kdl/frameacc.hpp> 40 #include <kdl/frames.hpp> 54 const geometry_msgs::WrenchStamped &ft_raw)
56 if(gravity.header.frame_id != ft_raw.header.frame_id)
58 ROS_ERROR(
"Gravity vector and ft raw expressed in different frames (%s, %s)!",
59 gravity.header.frame_id.c_str(), ft_raw.header.frame_id.c_str());
66 Eigen::VectorXd
z = Eigen::Matrix<double, 6, 1>::Zero();
67 z(0) = ft_raw.wrench.force.x;
68 z(1) = ft_raw.wrench.force.y;
69 z(2) = ft_raw.wrench.force.z;
71 z(3) = ft_raw.wrench.torque.x;
72 z(4) = ft_raw.wrench.torque.y;
73 z(5) = ft_raw.wrench.torque.z;
85 Eigen::MatrixXd H_temp =
H;
86 Eigen::VectorXd Z_temp =
Z;
105 Eigen::VectorXd ft_calib_params(10);
107 ft_calib_params =
H.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(
Z);
109 return ft_calib_params;
119 KDL::Vector g(gravity.vector.x, gravity.vector.y, gravity.vector.z);
122 H = Eigen::Matrix<double, 6, 10>::Zero();
124 for(
unsigned int i=0; i<3; i++)
126 for(
unsigned int j=4; j<10; j++)
139 for(
unsigned int i=3; i<6; i++)
148 for(
unsigned int i=0; i<3; i++)
150 H(i,0) = a(i) - g(i);
153 H(0,1) = -w(1)*w(1) - w(2)*w(2);
154 H(0,2) = w(0)*w(1) - alpha(2);
155 H(0,3) = w(0)*w(2) + alpha(1);
157 H(1,1) = w(0)*w(1) + alpha(2);
158 H(1,2) = -w(0)*w(0) - w(2)*w(2);
159 H(1,3) = w(1)*w(2) - alpha(0);
161 H(2,1) = w(0)*w(2) - alpha(1);
162 H(2,2) = w(1)*w(2) + alpha(0);
163 H(2,3) = -w(1)*w(1) - w(0)*w(0);
165 H(3,2) = a(2) - g(2);
166 H(3,3) = -a(1) + g(1);
169 H(4,1) = -a(2) + g(2);
170 H(4,3) = a(0) - g(0);
172 H(5,1) = a(1) - g(1);
173 H(5,2) = -a(0) + g(0);
175 for(
unsigned int i=3; i<6; i++)
177 for(
unsigned int j=4; j<10; j++)
virtual void addMeasurement(const geometry_msgs::Vector3Stamped &gravity, const geometry_msgs::WrenchStamped &ft_raw)
virtual Eigen::MatrixXd getMeasurementMatrix(const geometry_msgs::Vector3Stamped &gravity)
virtual Eigen::VectorXd getCalib()
TFSIMD_FORCE_INLINE const tfScalar & w() const