void FFTWidget::setRectWindow()
{
for(int i=0; i<1024;i++)
{
window_function[i]=f_offset(i, 1.0/32768.0);
}
current_window = FFTWidget::RectangularWindow;
}
void FFTWidget::setHannWindow()
{
for(int i=0; i<1024; i++)
{
window_function[i] = f_offset(i, 0.5*(1-cos((2.0*M_PI*i)/(1024-1)))/32768.0);
}
current_window = FFTWidget::HannWindow;
}
// receive FT Sensor data
void
recvFT(const geometry_msgs::WrenchStampedConstPtr& msg){
mutex_force.lock();
// std::cout <<"Fx"<<"\t"<<"Fy"<<"\t"<<"Fz"<<"\t"<<"Tx"<<"\t"<<"Ty"<<"\t"<<"Tz"<<std::endl;
// if (counter >50){
// std::cout << msg->wrench.force.x<<","<<msg->wrench.force.y<<","<<msg->wrench.force.z<<","<<msg->wrench.torque.x<<","<<msg->wrench.torque.y<<","<<msg->wrench.torque.z<<std::endl;
// counter = 0;
// }
// else{
// counter ++;
// }
Eigen::Vector3d grav_tmp;
grav_tmp.setZero();
grav_tmp(2) = Grav_Schunk_Acc+tool_grav;
ft_gama->raw_ft_f(0) = msg->wrench.force.x;
ft_gama->raw_ft_f(1) = msg->wrench.force.y;
ft_gama->raw_ft_f(2) = msg->wrench.force.z-Grav_Schunk_Acc;
ft_gama->raw_ft_t(0) = msg->wrench.torque.x;
ft_gama->raw_ft_t(1) = msg->wrench.torque.y;
ft_gama->raw_ft_t(2) = msg->wrench.torque.z;
//get rid of tool/pokingstick gravity--calib
ft_gama->calib_ft_f = right_rs->robot_orien["eef"] * ft_gama->raw_ft_f + grav_tmp;
ft_gama->calib_ft_t = ft_gama->raw_ft_t;
Eigen::Vector3d f_offset;
f_offset.setZero();
f_offset(0) = -0.5;
f_offset(1) = -0.5;
f_offset(2) = -0.6;
//use smooth filter to get rid of noise.
ft.head(3) = ft_gama->filtered_gama_f = gama_f_filter->push(ft_gama->calib_ft_f) - f_offset;
//get rid of noise of no contacting
if(ft_gama->filtered_gama_f.norm() <1.5)
ft.head(3).setZero();
ft.tail(3) = ft_gama->filtered_gama_t = gama_t_filter->push(ft_gama->calib_ft_t);
counter_t ++;
if(counter_t%50==0){
counter_t = 0;
// std::cout<<"estimated contact force: "<<ft_gama->filtered_gama_f(0)<<","<<ft_gama->filtered_gama_f(1)<<","
// <<ft_gama->filtered_gama_f(2)<<","<<ft_gama->filtered_gama_t(0)<<","<<ft_gama->filtered_gama_t(1)<<","<<ft_gama->filtered_gama_t(2)<<std::endl;
}
mutex_force.unlock();
}
void FFTWidget::setBlackmanHarrissWindow()
{
const float a0 = 0.35875;
const float a1 = 0.48829;
const float a2 = 0.14128;
const float a3 = 0.01168;
for(int i=0; i<1024; i++)
{
window_function[i] = f_offset(i, (a0 - a1*cos((2.0*M_PI*i)/(1024-1)) +a2*cos((4.0*M_PI*i)/(1024-1)) -a3*cos((6.0*M_PI*i)/(1024-1)))/32768.0);
}
current_window = FFTWidget::BlackmanHarrissWindow;
}
