static void apply(void *invec, void *inoutvec, int *len,
MPI_Datatype *datatype) {
functor local_f(*f);
T *i1=reinterpret_cast<T *>(invec);
T *i2=reinterpret_cast<T *>(inoutvec);
for (int i=0, i_end=*len; i<i_end; ++i, ++i1, ++i2)
*i2=local_f(*i1, *i2);
}
void record_training_cb(boost::shared_ptr<std::string> data){
Eigen::Vector3d local_f = InitOrien.transpose() * ft.head(3);
Eigen::Vector3d local_handle_dir = InitOrien.transpose() * right_rs->robot_orien["eef"].col(1);
Tpose<< local_f(0)<<","<< local_f(1)<<","<< local_f(2)<<","<<local_handle_dir(0)<<","<<local_handle_dir(1)<<","<<local_handle_dir(2)<<std::endl;
std::cout<<"recording training data"<<std::endl;
}
