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