From 2792d2689a958cd3dd592d0c98a1798d6515a865 Mon Sep 17 00:00:00 2001 From: mahmoudali Date: Thu, 17 Oct 2019 11:33:57 +0200 Subject: [PATCH 01/12] add orientation calibration node --- CMakeLists.txt | 7 + src/tcp_orientation_calibration.cpp | 252 ++++++++++++++++++++++++++++ 2 files changed, 259 insertions(+) create mode 100644 src/tcp_orientation_calibration.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index c6b7224..3e0b0c8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,6 +44,13 @@ target_link_libraries(console_calibration ) +#combined +add_executable(tcp_orientation_calibration src/tcp_orientation_calibration.cpp) +add_dependencies(tcp_orientation_calibration ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(tcp_orientation_calibration ${catkin_LIBRARIES} ${CERES_LIBRARIES} tool_point_calibration) + + + ############# ## Install ## ############# diff --git a/src/tcp_orientation_calibration.cpp b/src/tcp_orientation_calibration.cpp new file mode 100644 index 0000000..a86f6fd --- /dev/null +++ b/src/tcp_orientation_calibration.cpp @@ -0,0 +1,252 @@ +#include +#include +#include +#include + +#include +using namespace Eigen; +using Eigen::MatrixXd; + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "console_tool_calibration"); + ros::NodeHandle pnh ("~"); + + // Load user parmameters + std::string base_frame, tool0_frame; + int num_samples; + + pnh.param("base_frame", base_frame, "base_link"); + pnh.param("tool0_frame", tool0_frame, "tool0"); + pnh.param("num_samples", num_samples, 4); + + ROS_INFO("Starting tool calibration with base frame: '%s' and tool0 frame: '%s'.", + base_frame.c_str(), tool0_frame.c_str()); + ROS_INFO("\n ======= step 1: translation calibration ========== "); + ROS_INFO("Move the robot to '%d' different poses, each of which should touch" + " the tool to the same position in space.\n", num_samples); + + // Create a transform listener to query tool frames + tf::TransformListener listener; + + std::string error_msg; + if (!listener.waitForTransform(base_frame, tool0_frame, ros::Time(0), + ros::Duration(1.0), ros::Duration(0.01), + &error_msg)) + { + ROS_WARN_STREAM("Unable to lookup transform between base frame: '" << base_frame + << "' and tool frame: '" << tool0_frame << "'. TF reported error: " + << error_msg); + + bool base_found = listener.frameExists(base_frame); + bool tool_found = listener.frameExists(tool0_frame); + + if (!base_found && !tool_found) + { + ROS_WARN("Check to make sure that a robot state publisher or other node is publishing" + " tf frames for your robot. Also check that your base/tool frames names are" + " correct and not missing a prefix, for example."); + } + else if (!base_found) + { + ROS_WARN("Check to make sure that base frame '%s' actually exists.", base_frame.c_str()); + } + else if (!tool_found) + { + ROS_WARN("Check to make sure that tool0 frame '%s' actually exists.", tool0_frame.c_str()); + } + + return 1; + } + + // Create storage for user observations + tool_point_calibration::Affine3dVector observations; + observations.reserve(num_samples); + + std::string line; + int count = 0; + + // While ros is ok and there are more counts to be done... + while (ros::ok() && count < num_samples) + { + ROS_INFO("Pose %d: Jog robot to a new location touching the shared position and" + " press enter.", count); + + std::getline(std::cin, line); // Blocks program until enter is pressed + + tf::StampedTransform transform; + try + { + listener.lookupTransform(base_frame, tool0_frame, + ros::Time(0), transform); + + Eigen::Affine3d eigen_pose; + tf::poseTFToEigen(transform, eigen_pose); + + observations.push_back(eigen_pose); + + ROS_INFO_STREAM("Pose " << count << ": captured transform:\n" << eigen_pose.matrix()); + count++; + } + catch (const tf::TransformException& ex) + { + ROS_ERROR("%s",ex.what()); + ros::Duration(1.0).sleep(); + continue; + } + } + + ROS_INFO("Calibration captured %d tool poses (out of %d requested). Computing calibration...", + count, num_samples); + + tool_point_calibration::TcpCalibrationResult result = + tool_point_calibration::calibrateTcp(observations); + + ROS_INFO_STREAM("Calibrated tcp (meters in xyz): [" << result.tcp_offset.transpose() << "] from " << tool0_frame); + ROS_INFO_STREAM("Touch point (meters in xyz): [" << result.touch_point.transpose() << "] in frame " << base_frame); + ROS_INFO_STREAM("Average residual: " << result.average_residual); + ROS_INFO_STREAM("Converged: " << result.converged); + // store the translation vector for the next step: + Vector3d tcp_transl; + tcp_transl = result.tcp_offset.transpose(); + + + + if (count < 4) + { + ROS_WARN("Computing a tool calibration w/ fewer than 4 points may produce an answer with good" + " convergence and residual error, but without a meaningful result. You are encouraged" + " to try with more points"); + } + + + + + + ROS_INFO("=======\n step 2: TCP Orientation calibration using three point method ========== "); + std::vector A0n; // to store the transformation matrix for the points + A0n.resize( 3); + + + for (int i=0; i<3; i++) + { + + if(i ==0) + ROS_INFO("First point: keep the TCP at same point where the translation calibration was done, press enter"); + else if(i ==1) + ROS_INFO("Second point: move the TCP to any location along the X axis of the tool and press enter"); + else + ROS_INFO("Third Point: move TCP to any location along the Y axis of the tool and press enter."); + std::getline(std::cin, line); // Blocks program until enter is pressed + + + tf::StampedTransform transform; + try + { + listener.lookupTransform(base_frame, tool0_frame, ros::Time(0), transform); + Eigen::Affine3d eigen_pose; + tf::poseTFToEigen(transform, eigen_pose); // convert transform to Affine3d + A0n[count] = eigen_pose; + ROS_INFO_STREAM("Pose " << i << ": captured transform:\n" << eigen_pose.matrix()); + } + catch (const tf::TransformException& ex) + { + ROS_ERROR("%s",ex.what()); + ros::Duration(1.0).sleep(); + continue; + } + } + + // here we have the three transformation A0n_1st, A0n_2nd, A0n_3rd + Vector4d r0ns_n = {tcp_transl[0], tcp_transl[1], tcp_transl[2], 1}; //tool center popint vector from pkg_1 + Vector4d rd_0 = A0n[0]*r0ns_n; + Vector4d r0ns_0 = rd_0; + + + // point 2: calculate delat1 and then c11, c21, c31 + Vector4d V1;//{ vx, vy, vz, 1}; + V1 = A0n[1].inverse() * A0n[0] * r0ns_n; // r0ns_0 has been modified to r0ns_n + double term1 = ( V1(0) - r0ns_n(0) ); + double term2 = ( V1(1) - r0ns_n(1) ); + double term3 = ( V1(2) - r0ns_n(2) ); + double delta1 = -1* sqrt( term1*term1 + term2*term2 + term3*term3); + double c11 = term1 / delta1; + double c21 = term2 / delta1; + double c31 = term3 / delta1; + ROS_INFO_STREAM(" delat1: "<< delta1); + std::cout<<"c11, c21, c31: "<< c11 << " "<< c21 << " " < Date: Mon, 6 Apr 2020 17:28:09 +0200 Subject: [PATCH 02/12] remove white spaces --- CMakeLists.txt | 4 -- src/tcp_orientation_calibration.cpp | 66 ----------------------------- 2 files changed, 70 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3e0b0c8..2f52126 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,14 +43,10 @@ target_link_libraries(console_calibration tool_point_calibration ) - -#combined add_executable(tcp_orientation_calibration src/tcp_orientation_calibration.cpp) add_dependencies(tcp_orientation_calibration ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(tcp_orientation_calibration ${catkin_LIBRARIES} ${CERES_LIBRARIES} tool_point_calibration) - - ############# ## Install ## ############# diff --git a/src/tcp_orientation_calibration.cpp b/src/tcp_orientation_calibration.cpp index a86f6fd..4d2f0ae 100644 --- a/src/tcp_orientation_calibration.cpp +++ b/src/tcp_orientation_calibration.cpp @@ -7,29 +7,23 @@ using namespace Eigen; using Eigen::MatrixXd; - int main(int argc, char** argv) { ros::init(argc, argv, "console_tool_calibration"); ros::NodeHandle pnh ("~"); - // Load user parmameters std::string base_frame, tool0_frame; int num_samples; - pnh.param("base_frame", base_frame, "base_link"); pnh.param("tool0_frame", tool0_frame, "tool0"); pnh.param("num_samples", num_samples, 4); - ROS_INFO("Starting tool calibration with base frame: '%s' and tool0 frame: '%s'.", base_frame.c_str(), tool0_frame.c_str()); ROS_INFO("\n ======= step 1: translation calibration ========== "); ROS_INFO("Move the robot to '%d' different poses, each of which should touch" " the tool to the same position in space.\n", num_samples); - // Create a transform listener to query tool frames tf::TransformListener listener; - std::string error_msg; if (!listener.waitForTransform(base_frame, tool0_frame, ros::Time(0), ros::Duration(1.0), ros::Duration(0.01), @@ -38,10 +32,8 @@ int main(int argc, char** argv) ROS_WARN_STREAM("Unable to lookup transform between base frame: '" << base_frame << "' and tool frame: '" << tool0_frame << "'. TF reported error: " << error_msg); - bool base_found = listener.frameExists(base_frame); bool tool_found = listener.frameExists(tool0_frame); - if (!base_found && !tool_found) { ROS_WARN("Check to make sure that a robot state publisher or other node is publishing" @@ -56,14 +48,12 @@ int main(int argc, char** argv) { ROS_WARN("Check to make sure that tool0 frame '%s' actually exists.", tool0_frame.c_str()); } - return 1; } // Create storage for user observations tool_point_calibration::Affine3dVector observations; observations.reserve(num_samples); - std::string line; int count = 0; @@ -72,20 +62,15 @@ int main(int argc, char** argv) { ROS_INFO("Pose %d: Jog robot to a new location touching the shared position and" " press enter.", count); - std::getline(std::cin, line); // Blocks program until enter is pressed - tf::StampedTransform transform; try { listener.lookupTransform(base_frame, tool0_frame, ros::Time(0), transform); - Eigen::Affine3d eigen_pose; tf::poseTFToEigen(transform, eigen_pose); - observations.push_back(eigen_pose); - ROS_INFO_STREAM("Pose " << count << ": captured transform:\n" << eigen_pose.matrix()); count++; } @@ -96,13 +81,10 @@ int main(int argc, char** argv) continue; } } - ROS_INFO("Calibration captured %d tool poses (out of %d requested). Computing calibration...", count, num_samples); - tool_point_calibration::TcpCalibrationResult result = tool_point_calibration::calibrateTcp(observations); - ROS_INFO_STREAM("Calibrated tcp (meters in xyz): [" << result.tcp_offset.transpose() << "] from " << tool0_frame); ROS_INFO_STREAM("Touch point (meters in xyz): [" << result.touch_point.transpose() << "] in frame " << base_frame); ROS_INFO_STREAM("Average residual: " << result.average_residual); @@ -110,25 +92,16 @@ int main(int argc, char** argv) // store the translation vector for the next step: Vector3d tcp_transl; tcp_transl = result.tcp_offset.transpose(); - - - if (count < 4) { ROS_WARN("Computing a tool calibration w/ fewer than 4 points may produce an answer with good" " convergence and residual error, but without a meaningful result. You are encouraged" " to try with more points"); } - - - - - ROS_INFO("=======\n step 2: TCP Orientation calibration using three point method ========== "); std::vector A0n; // to store the transformation matrix for the points A0n.resize( 3); - for (int i=0; i<3; i++) { @@ -139,8 +112,6 @@ int main(int argc, char** argv) else ROS_INFO("Third Point: move TCP to any location along the Y axis of the tool and press enter."); std::getline(std::cin, line); // Blocks program until enter is pressed - - tf::StampedTransform transform; try { @@ -157,13 +128,10 @@ int main(int argc, char** argv) continue; } } - // here we have the three transformation A0n_1st, A0n_2nd, A0n_3rd Vector4d r0ns_n = {tcp_transl[0], tcp_transl[1], tcp_transl[2], 1}; //tool center popint vector from pkg_1 Vector4d rd_0 = A0n[0]*r0ns_n; Vector4d r0ns_0 = rd_0; - - // point 2: calculate delat1 and then c11, c21, c31 Vector4d V1;//{ vx, vy, vz, 1}; V1 = A0n[1].inverse() * A0n[0] * r0ns_n; // r0ns_0 has been modified to r0ns_n @@ -176,10 +144,6 @@ int main(int argc, char** argv) double c31 = term3 / delta1; ROS_INFO_STREAM(" delat1: "<< delta1); std::cout<<"c11, c21, c31: "<< c11 << " "<< c21 << " " < Date: Mon, 6 Apr 2020 17:30:27 +0200 Subject: [PATCH 03/12] remove white spaces --- src/tcp_orientation_calibration.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/tcp_orientation_calibration.cpp b/src/tcp_orientation_calibration.cpp index 4d2f0ae..34530e1 100644 --- a/src/tcp_orientation_calibration.cpp +++ b/src/tcp_orientation_calibration.cpp @@ -2,7 +2,6 @@ #include #include #include - #include using namespace Eigen; using Eigen::MatrixXd; @@ -50,13 +49,11 @@ int main(int argc, char** argv) } return 1; } - // Create storage for user observations tool_point_calibration::Affine3dVector observations; observations.reserve(num_samples); std::string line; int count = 0; - // While ros is ok and there are more counts to be done... while (ros::ok() && count < num_samples) { @@ -104,7 +101,6 @@ int main(int argc, char** argv) for (int i=0; i<3; i++) { - if(i ==0) ROS_INFO("First point: keep the TCP at same point where the translation calibration was done, press enter"); else if(i ==1) From cc34ff2b7ce9b1812572d8481fd35f2f5171d482 Mon Sep 17 00:00:00 2001 From: mahmoud-a-ali Date: Wed, 29 Apr 2020 19:08:18 +0200 Subject: [PATCH 04/12] PR review modification --- CMakeLists.txt | 6 +- ...n.cpp => tool_orientation_calibration.cpp} | 119 +++++++++--------- 2 files changed, 66 insertions(+), 59 deletions(-) rename src/{tcp_orientation_calibration.cpp => tool_orientation_calibration.cpp} (65%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2f52126..f77a229 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,9 +43,9 @@ target_link_libraries(console_calibration tool_point_calibration ) -add_executable(tcp_orientation_calibration src/tcp_orientation_calibration.cpp) -add_dependencies(tcp_orientation_calibration ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(tcp_orientation_calibration ${catkin_LIBRARIES} ${CERES_LIBRARIES} tool_point_calibration) +add_executable(tool_orientation_calibration src/tool_orientation_calibration.cpp) +add_dependencies(tool_orientation_calibration ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(tool_orientation_calibration ${catkin_LIBRARIES} ${CERES_LIBRARIES} tool_point_calibration) ############# ## Install ## diff --git a/src/tcp_orientation_calibration.cpp b/src/tool_orientation_calibration.cpp similarity index 65% rename from src/tcp_orientation_calibration.cpp rename to src/tool_orientation_calibration.cpp index 34530e1..5daaded 100644 --- a/src/tcp_orientation_calibration.cpp +++ b/src/tool_orientation_calibration.cpp @@ -5,10 +5,18 @@ #include using namespace Eigen; using Eigen::MatrixXd; - +/** +this node is used to calibrate the orientation of the tool point. +it is an implementation of a paper entitled: +Methods of Calibrating the Orientation of the Industrial Robot Tool[1]. +variables names are same as the ones used in the paper. +------------------- +[1]https://www.researchgate.net/publication/330247404_ +Methods_of_Calibrating_the_Orientation_of_the_Industrial_Robot_Tool +**/ int main(int argc, char** argv) { - ros::init(argc, argv, "console_tool_calibration"); + ros::init(argc, argv, "tool_orientation_calibration"); ros::NodeHandle pnh ("~"); // Load user parmameters std::string base_frame, tool0_frame; @@ -124,59 +132,58 @@ int main(int argc, char** argv) continue; } } - // here we have the three transformation A0n_1st, A0n_2nd, A0n_3rd - Vector4d r0ns_n = {tcp_transl[0], tcp_transl[1], tcp_transl[2], 1}; //tool center popint vector from pkg_1 - Vector4d rd_0 = A0n[0]*r0ns_n; - Vector4d r0ns_0 = rd_0; - // point 2: calculate delat1 and then c11, c21, c31 - Vector4d V1;//{ vx, vy, vz, 1}; - V1 = A0n[1].inverse() * A0n[0] * r0ns_n; // r0ns_0 has been modified to r0ns_n - double term1 = ( V1(0) - r0ns_n(0) ); - double term2 = ( V1(1) - r0ns_n(1) ); - double term3 = ( V1(2) - r0ns_n(2) ); - double delta1 = -1* sqrt( term1*term1 + term2*term2 + term3*term3); - double c11 = term1 / delta1; - double c21 = term2 / delta1; - double c31 = term3 / delta1; - ROS_INFO_STREAM(" delat1: "<< delta1); - std::cout<<"c11, c21, c31: "<< c11 << " "<< c21 << " " < Date: Thu, 30 Apr 2020 18:50:12 +0200 Subject: [PATCH 05/12] change to isometry --- include/tool_point_calibration/tool_point_calibration.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/include/tool_point_calibration/tool_point_calibration.h b/include/tool_point_calibration/tool_point_calibration.h index 1b6f481..7452a35 100644 --- a/include/tool_point_calibration/tool_point_calibration.h +++ b/include/tool_point_calibration/tool_point_calibration.h @@ -2,11 +2,12 @@ #define TOOL_POINT_CALIBRATION_H #include +#include namespace tool_point_calibration { -typedef std::vector> Affine3dVector; +typedef std::vector> Isometry3dVector; struct TcpCalibrationResult { @@ -40,7 +41,7 @@ struct TcpCalibrationResult * @param touch_pt_guess * @return */ -TcpCalibrationResult calibrateTcp(const Affine3dVector& tool_poses, +TcpCalibrationResult calibrateTcp(const Isometry3dVector& tool_poses, const Eigen::Vector3d& tcp_guess = Eigen::Vector3d::Zero(), const Eigen::Vector3d& touch_pt_guess = Eigen::Vector3d::Zero()); From c333cd7a53e3b86f794c8c9b265c538dd1a8aa9c Mon Sep 17 00:00:00 2001 From: mahmoud-a-ali Date: Thu, 30 Apr 2020 18:50:23 +0200 Subject: [PATCH 06/12] change to isometry --- src/console_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/console_calibration.cpp b/src/console_calibration.cpp index 9ee77e1..a2a90ad 100644 --- a/src/console_calibration.cpp +++ b/src/console_calibration.cpp @@ -55,7 +55,7 @@ int main(int argc, char** argv) } // Create storage for user observations - tool_point_calibration::Affine3dVector observations; + tool_point_calibration::Isometry3dVector observations; observations.reserve(num_samples); std::string line; @@ -75,7 +75,7 @@ int main(int argc, char** argv) listener.lookupTransform(base_frame, tool0_frame, ros::Time(0), transform); - Eigen::Affine3d eigen_pose; + Eigen::Isometry3d eigen_pose; tf::poseTFToEigen(transform, eigen_pose); observations.push_back(eigen_pose); From b877acc7c010ae1eca840c2e3c355cc257a54e75 Mon Sep 17 00:00:00 2001 From: mahmoud-a-ali Date: Thu, 30 Apr 2020 18:50:39 +0200 Subject: [PATCH 07/12] change to isometry --- src/tool_orientation_calibration.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/tool_orientation_calibration.cpp b/src/tool_orientation_calibration.cpp index 5daaded..b982651 100644 --- a/src/tool_orientation_calibration.cpp +++ b/src/tool_orientation_calibration.cpp @@ -58,7 +58,7 @@ int main(int argc, char** argv) return 1; } // Create storage for user observations - tool_point_calibration::Affine3dVector observations; + tool_point_calibration::Isometry3dVector observations; observations.reserve(num_samples); std::string line; int count = 0; @@ -73,7 +73,7 @@ int main(int argc, char** argv) { listener.lookupTransform(base_frame, tool0_frame, ros::Time(0), transform); - Eigen::Affine3d eigen_pose; + Eigen::Isometry3d eigen_pose; tf::poseTFToEigen(transform, eigen_pose); observations.push_back(eigen_pose); ROS_INFO_STREAM("Pose " << count << ": captured transform:\n" << eigen_pose.matrix()); @@ -104,7 +104,7 @@ int main(int argc, char** argv) " to try with more points"); } ROS_INFO("=======\n step 2: TCP Orientation calibration using three point method ========== "); - std::vector A0n; // to store the transformation matrix for the points + std::vector A0n; // to store the transformation matrix for the points A0n.resize( 3); for (int i=0; i<3; i++) @@ -120,9 +120,9 @@ int main(int argc, char** argv) try { listener.lookupTransform(base_frame, tool0_frame, ros::Time(0), transform); - Eigen::Affine3d eigen_pose; - tf::poseTFToEigen(transform, eigen_pose); // convert transform to Affine3d - A0n[count] = eigen_pose; + Eigen::Isometry3d eigen_pose; + tf::poseTFToEigen(transform, eigen_pose); + A0n[i] = eigen_pose; ROS_INFO_STREAM("Pose " << i << ": captured transform:\n" << eigen_pose.matrix()); } catch (const tf::TransformException& ex) @@ -147,7 +147,7 @@ int main(int argc, char** argv) double c11 = term1 / delta1; double c21 = term2 / delta1; double c31 = term3 / delta1; - ROS_INFO_STREAM(" delat1: ", delta1); + ROS_INFO_STREAM(" delat1: "<< delta1); ROS_INFO_STREAM("c11: "<< c11 << ", c21: "<< c21 << ", c31: "< Date: Thu, 30 Apr 2020 18:50:47 +0200 Subject: [PATCH 08/12] change to isometry --- src/tool_point_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/tool_point_calibration.cpp b/src/tool_point_calibration.cpp index 37c7996..a95dcc0 100644 --- a/src/tool_point_calibration.cpp +++ b/src/tool_point_calibration.cpp @@ -8,7 +8,7 @@ */ struct ToolPointEstimator { - ToolPointEstimator(const Eigen::Affine3d& robot_pose) + ToolPointEstimator(const Eigen::Isometry3d& robot_pose) { ceres::RotationMatrixToAngleAxis(robot_pose.rotation().data(), angle_axis_); translation_[0] = robot_pose.translation()(0); @@ -48,7 +48,7 @@ struct ToolPointEstimator tool_point_calibration::TcpCalibrationResult -tool_point_calibration::calibrateTcp(const tool_point_calibration::Affine3dVector &tool_poses, +tool_point_calibration::calibrateTcp(const tool_point_calibration::Isometry3dVector &tool_poses, const Eigen::Vector3d &tcp_guess, const Eigen::Vector3d &touch_pt_guess) { From 7541f5ad0db8a6a4f9661d665f7620781012aab4 Mon Sep 17 00:00:00 2001 From: mahmoud-a-ali Date: Thu, 30 Apr 2020 20:11:39 +0200 Subject: [PATCH 09/12] modify README --- README.md | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 73be57e..18c682d 100644 --- a/README.md +++ b/README.md @@ -7,7 +7,7 @@ The core tcp calibration library only requires the non-linear optimization solve The console calibration utility uses ROS' TF libraries. ## Use -To use: +### tool_point_calibration: 1. Start up your robots driver as normal. A node like `robot_state_publisher` should be publishing TF frames of your robot. 2. Identify a base frame, either the base frame of the robot or the origin of the scene. This will be used for TF lookups, and the `touch point` will be reported in this frame. @@ -25,4 +25,27 @@ To use: 6. Repeat step 5 at least three more times. Each time approach the fixed point in the world from a different orientation. If you wish to abort early, control-C the node. - 7. When you've captured all of the required points, the calibration will be automatically run and the output displayed to the screen. \ No newline at end of file + 7. When you've captured all of the required points, the calibration will be automatically run and the output displayed to the screen. + + +### tool_orientation_calibration: + 1. repeat the first three steps in the case of tool_point_calibration. + + 2. Run the orientation node: + ``` + roslaunch tool_point_calibration orientation_calibration.launch base_frame:=YOUR_BASE_FRAME tool0_frame:=YOUR_TOOL0_FRAME num_samples:=NUMBER_OF_TOUCH_POINTS + ``` + By default base frame is `base_frame`, tool0_frame is `tool0`, and num_samples is `4`. I recommend you use at least 4 + samples. + + 3. Repeat steps 5,6,7 of the tool_point_calibration case. + + 4. The last pose that has been used for translational calibration will be automatically saved as the first poin for the orientation calibration, two more points are requied. + + 5. Jog the robot in such away that the TCP moves to any location along the X axis of the tool and press enter. + + 6. Jog the robot in such away that the TCP moves to any location along the Y axis of the tool and press enter. + + + 7. When you've captured all of the required points, the orientation calibration will be automatically run and the output displayed to the screen. + From fc86b7abf64c2a2d32b8c4e3802a6afa96851621 Mon Sep 17 00:00:00 2001 From: mahmoud-a-ali Date: Thu, 30 Apr 2020 20:11:55 +0200 Subject: [PATCH 10/12] add launch file --- launch/orientation_calibration.launch | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 launch/orientation_calibration.launch diff --git a/launch/orientation_calibration.launch b/launch/orientation_calibration.launch new file mode 100644 index 0000000..64705f1 --- /dev/null +++ b/launch/orientation_calibration.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + From 594e159864742f5d5a59591f865df784ec04c0dc Mon Sep 17 00:00:00 2001 From: mahmoud-a-ali Date: Thu, 30 Apr 2020 20:12:22 +0200 Subject: [PATCH 11/12] reduce for loop to 2 steps --- src/tool_orientation_calibration.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/tool_orientation_calibration.cpp b/src/tool_orientation_calibration.cpp index b982651..dd12e4a 100644 --- a/src/tool_orientation_calibration.cpp +++ b/src/tool_orientation_calibration.cpp @@ -106,11 +106,10 @@ int main(int argc, char** argv) ROS_INFO("=======\n step 2: TCP Orientation calibration using three point method ========== "); std::vector A0n; // to store the transformation matrix for the points A0n.resize( 3); - - for (int i=0; i<3; i++) + A0n[i] = eigen_pose; + ROS_INFO("the current pose is considered as the First point ... "); + for (int i=1; i<3; i++) { - if(i ==0) - ROS_INFO("First point: keep the TCP at same point where the translation calibration was done, press enter"); else if(i ==1) ROS_INFO("Second point: move the TCP to any location along the X axis of the tool and press enter"); else From 602d69581308bb43942200f5b51c416e3f128abc Mon Sep 17 00:00:00 2001 From: mahmoud-a-ali Date: Thu, 30 Apr 2020 20:25:39 +0200 Subject: [PATCH 12/12] reduce for loop to 2 steps --- src/tool_orientation_calibration.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/tool_orientation_calibration.cpp b/src/tool_orientation_calibration.cpp index dd12e4a..88be7cb 100644 --- a/src/tool_orientation_calibration.cpp +++ b/src/tool_orientation_calibration.cpp @@ -62,6 +62,7 @@ int main(int argc, char** argv) observations.reserve(num_samples); std::string line; int count = 0; + Eigen::Isometry3d eigen_pose; // While ros is ok and there are more counts to be done... while (ros::ok() && count < num_samples) { @@ -73,7 +74,6 @@ int main(int argc, char** argv) { listener.lookupTransform(base_frame, tool0_frame, ros::Time(0), transform); - Eigen::Isometry3d eigen_pose; tf::poseTFToEigen(transform, eigen_pose); observations.push_back(eigen_pose); ROS_INFO_STREAM("Pose " << count << ": captured transform:\n" << eigen_pose.matrix()); @@ -106,11 +106,11 @@ int main(int argc, char** argv) ROS_INFO("=======\n step 2: TCP Orientation calibration using three point method ========== "); std::vector A0n; // to store the transformation matrix for the points A0n.resize( 3); - A0n[i] = eigen_pose; + A0n[0] = eigen_pose; ROS_INFO("the current pose is considered as the First point ... "); for (int i=1; i<3; i++) { - else if(i ==1) + if(i ==1) ROS_INFO("Second point: move the TCP to any location along the X axis of the tool and press enter"); else ROS_INFO("Third Point: move TCP to any location along the Y axis of the tool and press enter.");