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 <iostream>
00038 #include <sstream>
00039 #include <tf/tf.h>
00040 #include <tf/transform_listener.h>
00041 #include <tf/transform_datatypes.h>
00042 #include <tf_conversions/tf_eigen.h>
00043 #include <eigen_conversions/eigen_msg.h>
00044 #include <geometry_msgs/Pose.h>
00045 #include <geometry_msgs/WrenchStamped.h>
00046 #include <sensor_msgs/Imu.h>
00047 #include <moveit/move_group_interface/move_group.h>
00048 #include <force_torque_sensor_calib/ft_calib.h>
00049 #include <eigen3/Eigen/Core>
00050
00051 using namespace Calibration;
00052
00053
00054 class FTCalibNode
00055 {
00056
00057
00058 public:
00059 ros::NodeHandle n_;
00060 ros::AsyncSpinner *spinner;
00061 ros::Subscriber topicSub_ft_raw_;
00062 ros::Subscriber topicSub_Accelerometer_;
00063
00064 FTCalibNode()
00065 {
00066 n_ = ros::NodeHandle("~");
00067 spinner = new ros::AsyncSpinner(1);
00068 spinner->start();
00069
00070
00071
00072 topicSub_ft_raw_ = n_.subscribe("ft_raw", 1, &FTCalibNode::topicCallback_ft_raw, this);
00073 topicSub_Accelerometer_ = n_.subscribe("imu", 1, &FTCalibNode::topicCallback_imu, this);
00074
00075 m_pose_counter = 0;
00076 m_ft_counter = 0;
00077
00078 m_received_ft = false;
00079 m_received_imu = false;
00080
00081 m_finished = false;
00082 m_tf_listener = new tf::TransformListener();
00083
00084 m_ft_calib = new FTCalib();
00085 }
00086
00087 ~FTCalibNode()
00088 {
00089 saveCalibData();
00090 delete spinner;
00091 delete m_group;
00092 delete m_ft_calib;
00093 delete m_tf_listener;
00094 }
00095
00096
00097 bool getROSParameters()
00098 {
00099
00100
00101 if(n_.hasParam("moveit_group_name"))
00102 {
00103 n_.getParam("moveit_group_name", m_moveit_group_name);
00104 }
00105
00106 else
00107 {
00108 ROS_ERROR("No moveit_group_name parameter, shutting down node...");
00109 n_.shutdown();
00110 return false;
00111 }
00112
00113
00114 if(n_.hasParam("calib_file_name"))
00115 {
00116 n_.getParam("calib_file_name", m_calib_file_name);
00117 }
00118
00119 else
00120 {
00121 ROS_WARN("No calib_file_name parameter, setting to default 'ft_calib.yaml'");
00122 m_calib_file_name = std::string("ft_calib_data.yaml");
00123 }
00124
00125
00126
00127 if(n_.hasParam("calib_file_dir"))
00128 {
00129 n_.getParam("calib_file_dir", m_calib_file_dir);
00130 }
00131
00132 else
00133 {
00134 ROS_WARN("No calib_file_dir parameter, setting to default '~/.ros/ft_calib' ");
00135 m_calib_file_dir = std::string("~/.ros/ft_calib");
00136 }
00137
00138 if(n_.hasParam("poses_frame_id"))
00139 {
00140 n_.getParam("poses_frame_id", m_poses_frame_id);
00141 }
00142
00143 else
00144 {
00145 ROS_ERROR("No poses_frame_id parameter, shutting down node ...");
00146 n_.shutdown();
00147 return false;
00148 }
00149
00150
00151
00152 n_.param("random_poses", m_random_poses, false);
00153
00154
00155 n_.param("number_random_poses", m_number_random_poses, 30);
00156
00157
00158 return true;
00159 }
00160
00161 void init()
00162 {
00163 m_group = new move_group_interface::MoveGroup(m_moveit_group_name);
00164 }
00165
00166
00167
00168 bool moveNextPose()
00169 {
00170
00171 std::stringstream ss;
00172 ss << m_pose_counter;
00173 Eigen::Matrix<double, 6, 1> pose;
00174
00175
00176
00177
00178 if(!m_random_poses)
00179 {
00180 if(!getPose("pose"+ss.str(), pose))
00181 {
00182 ROS_INFO("Finished group %s poses", m_group->getName().c_str());
00183 m_finished = true;
00184 return true;
00185 }
00186
00187 geometry_msgs::Pose pose_;
00188 pose_.position.x = pose(0);
00189 pose_.position.y = pose(1);
00190 pose_.position.z = pose(2);
00191
00192 tf::Quaternion q;
00193 q.setRPY((double)pose(3), (double)pose(4), (double)pose(5));
00194
00195 tf::quaternionTFToMsg(q, pose_.orientation);
00196
00197 geometry_msgs::PoseStamped pose_stamped;
00198 pose_stamped.pose = pose_;
00199 pose_stamped.header.frame_id = m_poses_frame_id;
00200 pose_stamped.header.stamp = ros::Time::now();
00201
00202 m_group->setPoseTarget(pose_stamped);
00203
00204 }
00205 else
00206 {
00207 if(m_pose_counter<m_number_random_poses)
00208 {
00209 m_group->setRandomTarget();
00210 ROS_INFO("Executing pose %d",m_pose_counter);
00211 }
00212
00213 else
00214 {
00215 ROS_INFO("Finished group %s random poses", m_group->getName().c_str());
00216 m_finished = true;
00217 return true;
00218 }
00219 }
00220
00221
00222 m_pose_counter++;
00223 m_group->move();
00224 ROS_INFO("Finished executing pose %d", m_pose_counter-1);
00225 return true;
00226 }
00227
00228
00229
00230 bool getPose(const std::string &pose_param_name, Eigen::Matrix<double, 6, 1> &pose)
00231 {
00232 XmlRpc::XmlRpcValue PoseXmlRpc;
00233 if(n_.hasParam(pose_param_name))
00234 {
00235 n_.getParam(pose_param_name, PoseXmlRpc);
00236 }
00237
00238 else
00239 {
00240 ROS_WARN("Pose parameter %s not found", pose_param_name.c_str());
00241 return false;
00242 }
00243
00244 if(PoseXmlRpc.size()!=6)
00245 {
00246 ROS_ERROR("Pose parameter %s wrong size (must be 6)", pose_param_name.c_str());
00247 return false;
00248 }
00249
00250 for(unsigned int i=0; i<6; i++)
00251 pose(i) = (double)PoseXmlRpc[i];
00252
00253 return true;
00254 }
00255
00256
00257
00258 void saveCalibData()
00259 {
00260 double mass;
00261 Eigen::Vector3d COM_pos;
00262 Eigen::Vector3d f_bias;
00263 Eigen::Vector3d t_bias;
00264
00265 getCalib(mass, COM_pos, f_bias, t_bias);
00266
00267 XmlRpc::XmlRpcValue bias;
00268 bias.setSize(6);
00269 for(unsigned int i=0; i<3; i++)
00270 bias[i] = (double)f_bias(i);
00271
00272 for(unsigned int i=0; i<3; i++)
00273 bias[i+3] = (double)t_bias(i);
00274
00275 XmlRpc::XmlRpcValue COM_pose;
00276 COM_pose.setSize(6);
00277 for(unsigned int i=0; i<3; i++)
00278 COM_pose[i] = (double)COM_pos(i);
00279
00280 for(unsigned int i=0; i<3; i++)
00281 COM_pose[i+3] = 0.0;
00282
00283
00284 n_.setParam("/ft_calib/bias", bias);
00285 n_.setParam("/ft_calib/gripper_mass", mass);
00286 n_.setParam("/ft_calib/gripper_com_frame_id", m_ft_raw.header.frame_id.c_str());
00287 n_.setParam("/ft_calib/gripper_com_pose", COM_pose);
00288
00289
00290 std::string file = m_calib_file_dir + std::string("/") + m_calib_file_name;
00291
00292
00293 std::string command = std::string("mkdir -p ") + m_calib_file_dir;
00294 std::system(command.c_str());
00295
00296
00297 command.clear();
00298 command = std::string("rosparam dump ") + file + std::string(" /ft_calib");
00299 std::system(command.c_str());
00300 }
00301
00302
00303 bool finished()
00304 {
00305 return(m_finished);
00306 }
00307
00308 void topicCallback_ft_raw(const geometry_msgs::WrenchStamped::ConstPtr &msg)
00309 {
00310 ROS_DEBUG("In ft sensorcallback");
00311 m_ft_raw = *msg;
00312 m_received_ft = true;
00313 }
00314
00315
00316
00317 void topicCallback_imu(const sensor_msgs::Imu::ConstPtr &msg)
00318 {
00319 ROS_DEBUG("In accelerometer read callback");
00320
00321 m_imu= *msg;
00322 m_received_imu = true;
00323 }
00324
00325 void addMeasurement()
00326 {
00327
00328 m_ft_avg.wrench.force.x = -m_ft_avg.wrench.force.x/(double)m_ft_counter;
00329 m_ft_avg.wrench.force.y = -m_ft_avg.wrench.force.y/(double)m_ft_counter;
00330 m_ft_avg.wrench.force.z = -m_ft_avg.wrench.force.z/(double)m_ft_counter;
00331
00332 m_ft_avg.wrench.torque.x = -m_ft_avg.wrench.torque.x/(double)m_ft_counter;
00333 m_ft_avg.wrench.torque.y = -m_ft_avg.wrench.torque.y/(double)m_ft_counter;
00334 m_ft_avg.wrench.torque.z = -m_ft_avg.wrench.torque.z/(double)m_ft_counter;
00335
00336
00337 m_ft_counter = 0;
00338
00339 if(!m_received_ft)
00340 {
00341 ROS_ERROR("Haven't received F/T sensor measurements");
00342 return;
00343 }
00344
00345 if(!m_received_imu)
00346 {
00347 ROS_ERROR("Haven't received accelerometer readings");
00348 return;
00349 }
00350
00351
00352 geometry_msgs::Vector3Stamped gravity;
00353 gravity.header.stamp = ros::Time();
00354 gravity.header.frame_id = m_imu.header.frame_id;
00355 gravity.vector = m_imu.linear_acceleration;
00356
00357 geometry_msgs::Vector3Stamped gravity_ft_frame;
00358
00359 try
00360 {
00361 m_tf_listener->transformVector(m_ft_raw.header.frame_id, gravity, gravity_ft_frame);
00362 }
00363
00364 catch(tf::TransformException &ex)
00365 {
00366 ROS_ERROR("Error transforming accelerometer reading to the F/T sensor frame");
00367 ROS_ERROR("%s.", ex.what());
00368 return;
00369 }
00370
00371 m_ft_calib->addMeasurement(gravity_ft_frame, m_ft_avg);
00372 }
00373
00374 void getCalib(double &mass, Eigen::Vector3d &COM_pos, Eigen::Vector3d &f_bias, Eigen::Vector3d &t_bias)
00375 {
00376
00377 Eigen::VectorXd ft_calib = m_ft_calib->getCalib();
00378
00379 mass = ft_calib(0);
00380 if(mass<=0.0)
00381 {
00382 ROS_ERROR("Error in estimated mass (<= 0)");
00383
00384 }
00385
00386 Eigen::Vector3d center_mass_position(ft_calib(1)/mass,
00387 ft_calib(2)/mass,
00388 ft_calib(3)/mass);
00389
00390 COM_pos = center_mass_position;
00391
00392 f_bias(0) = -ft_calib(4);
00393 f_bias(1) = -ft_calib(5);
00394 f_bias(2) = -ft_calib(6);
00395 t_bias(0) = -ft_calib(7);
00396 t_bias(1) = -ft_calib(8);
00397 t_bias(2) = -ft_calib(9);
00398
00399 }
00400
00401 void averageFTMeas()
00402 {
00403
00404 if(m_ft_counter==0)
00405 {
00406
00407 m_ft_avg = m_ft_raw;
00408
00409 }
00410
00411 else
00412 {
00413
00414 m_ft_avg.wrench.force.x = m_ft_avg.wrench.force.x + m_ft_raw.wrench.force.x;
00415 m_ft_avg.wrench.force.y = m_ft_avg.wrench.force.y + m_ft_raw.wrench.force.y;
00416 m_ft_avg.wrench.force.z = m_ft_avg.wrench.force.z + m_ft_raw.wrench.force.z;
00417
00418 m_ft_avg.wrench.torque.x = m_ft_avg.wrench.torque.x + m_ft_raw.wrench.torque.x;
00419 m_ft_avg.wrench.torque.y = m_ft_avg.wrench.torque.y + m_ft_raw.wrench.torque.y;
00420 m_ft_avg.wrench.torque.z = m_ft_avg.wrench.torque.z + m_ft_raw.wrench.torque.z;
00421
00422 }
00423 m_ft_counter++;
00424 }
00425
00426
00427 private:
00428
00429 move_group_interface::MoveGroup *m_group;
00430
00431 unsigned int m_pose_counter;
00432 unsigned int m_ft_counter;
00433
00434 bool m_finished;
00435
00436 bool m_received_ft;
00437 bool m_received_imu;
00438
00439
00440 FTCalib *m_ft_calib;
00441
00442
00443 geometry_msgs::WrenchStamped m_ft_raw;
00444 geometry_msgs::WrenchStamped m_ft_avg;
00445
00446
00447 sensor_msgs::Imu m_imu;
00448
00449 tf::TransformListener *m_tf_listener;
00450
00451
00452
00453 std::string m_moveit_group_name;
00454
00455
00456 std::string m_calib_file_name;
00457
00458
00459
00460 std::string m_calib_file_dir;
00461
00462
00463 std::string m_poses_frame_id;
00464
00465
00466
00467 bool m_random_poses;
00468
00469
00470
00471 int m_number_random_poses;
00472
00473 };
00474
00475 int main(int argc, char **argv)
00476 {
00477 ros::init (argc, argv, "ft_calib_node");
00478 ros::NodeHandle nh;
00479
00480 FTCalibNode ft_calib_node;
00481 if(!ft_calib_node.getROSParameters())
00482 {
00483 ft_calib_node.n_.shutdown();
00484 ROS_ERROR("Error getting ROS parameters");
00485
00486 }
00487 ft_calib_node.init();
00488
00490 double loop_rate_;
00491 ft_calib_node.n_.param("loop_rate", loop_rate_, 650.0);
00492 ros::Rate loop_rate(loop_rate_);
00493
00494
00495 double wait_time;
00496 ft_calib_node.n_.param("wait_time", wait_time, 4.0);
00497
00498 bool ret = false;
00499 unsigned int n_measurements = 0;
00500
00501 ros::Time t_end_move_arm = ros::Time::now();
00502
00503 while (ft_calib_node.n_.ok() && !ft_calib_node.finished())
00504 {
00505
00506
00507 if(!ret)
00508 {
00509 ret = ft_calib_node.moveNextPose();
00510 t_end_move_arm = ros::Time::now();
00511 }
00512
00513
00514 else if ((ros::Time::now() - t_end_move_arm).toSec() > wait_time)
00515 {
00516 n_measurements++;
00517 ft_calib_node.averageFTMeas();
00518
00519 if(n_measurements==100)
00520 {
00521 ret = false;
00522 n_measurements = 0;
00523
00524 ft_calib_node.addMeasurement();
00525 double mass;
00526 Eigen::Vector3d COM_pos;
00527 Eigen::Vector3d f_bias;
00528 Eigen::Vector3d t_bias;
00529
00530 ft_calib_node.getCalib(mass, COM_pos, f_bias, t_bias);
00531 std::cout << "-------------------------------------------------------------" << std::endl;
00532 std::cout << "Current calibration estimate:" << std::endl;
00533 std::cout << std::endl << std::endl;
00534
00535 std::cout << "Mass: " << mass << std::endl << std::endl;
00536
00537 std::cout << "Center of mass position (relative to FT sensor frame):" << std::endl;
00538 std::cout << "[" << COM_pos(0) << ", " << COM_pos(1) << ", " << COM_pos(2) << "]";
00539 std::cout << std::endl << std::endl;
00540
00541
00542 std::cout << "FT bias: " << std::endl;
00543 std::cout << "[" << f_bias(0) << ", " << f_bias(1) << ", " << f_bias(2) << ", ";
00544 std::cout << t_bias(0) << ", " << t_bias(1) << ", " << t_bias(2) << "]";
00545 std::cout << std::endl << std::endl;
00546
00547
00548 std::cout << "-------------------------------------------------------------" << std::endl << std::endl << std::endl;
00549 ft_calib_node.saveCalibData();
00550 }
00551
00552 }
00553
00554
00555 ros::spinOnce();
00556 loop_rate.sleep();
00557 }
00558
00559 ft_calib_node.saveCalibData();
00560 ros::shutdown();
00561 return 0;
00562 }