From 1206282c3e70bac0f7d032fa33255c854ecca569 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 28 Jul 2022 21:25:50 +0800 Subject: [PATCH 1/3] Estimate linear joint value from cartesian pose Signed-off-by: Yadunund --- src/egm_common_auxiliary.cpp | 30 ++++++++++++++++++++++++++++++ src/egm_controller_interface.cpp | 10 ++++++++++ 2 files changed, 40 insertions(+) diff --git a/src/egm_common_auxiliary.cpp b/src/egm_common_auxiliary.cpp index 5316b04..c2828e3 100644 --- a/src/egm_common_auxiliary.cpp +++ b/src/egm_common_auxiliary.cpp @@ -866,6 +866,20 @@ bool parse(wrapper::Feedback* p_target, const EgmFeedBack& source, const RobotAx { bool success = false; + std::cout << "EGMFeedback joints: "; + for (const auto& j : source.joints().joints()) + std::cout << j << ","; + std::cout << std::endl; + + // std::cout << "EGMFeedback external joints: "; + // for (const auto& j : source.externalJoints().joints()) + // std::cout << j << ","; + // std::cout << std::endl; + + std::cout << "EGMFeedback pose: " << source.cartesian().pos().x() << "," + << source.cartesian().pos().y() << "," + << source.cartesian().pos().z() << std::endl; + if (p_target) { success = parse(p_target->mutable_robot()->mutable_joints()->mutable_position(), @@ -881,6 +895,22 @@ bool parse(wrapper::Feedback* p_target, const EgmFeedBack& source, const RobotAx else { success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian()); + + // This is a special case where the joint value for linear axis as reported by the robot is incorrect + // Hence we estimate this from the cartesian Z-Axis. This works for the IRB910SC 4-Axis robot + if (axes == Four) + { + if (source.has_cartesian()) + { + auto mutable_values = p_target->mutable_robot()->mutable_joints()->mutable_position()->mutable_values(); + auto& val = mutable_values->at(2); + val = source.cartesian().pos().z() - 220.2; + } + else + { + success = false; + } + } } if (success) diff --git a/src/egm_controller_interface.cpp b/src/egm_controller_interface.cpp index b8c503e..3a832e2 100644 --- a/src/egm_controller_interface.cpp +++ b/src/egm_controller_interface.cpp @@ -119,6 +119,16 @@ void EGMControllerInterface::ControllerMotion::readInputs(wrapper::Input* p_inpu { boost::lock_guard lock(read_mutex_); + std::cout << "### [ControllerMotion::readInputs()] ###" << std::endl; + std::cout << "Feedback: " << std::endl; + const auto& joint_space = inputs_.feedback().robot().joints(); + const auto& cartesian_space = inputs_.feedback().robot().cartesian(); + std::cout << "Positions: ["; + for (const auto& p : joint_space.position().values()) + { + std::cout << p << ","; + } + std::cout << "]" << std::endl; p_inputs->CopyFrom(inputs_); read_data_ready_ = false; } From 1287847528a9180ac5534409c02ad275af1aedd0 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 28 Jul 2022 22:58:06 +0800 Subject: [PATCH 2/3] Cleanup Signed-off-by: Yadunund --- src/egm_common_auxiliary.cpp | 14 -------------- src/egm_controller_interface.cpp | 10 ---------- 2 files changed, 24 deletions(-) diff --git a/src/egm_common_auxiliary.cpp b/src/egm_common_auxiliary.cpp index c2828e3..774c725 100644 --- a/src/egm_common_auxiliary.cpp +++ b/src/egm_common_auxiliary.cpp @@ -866,20 +866,6 @@ bool parse(wrapper::Feedback* p_target, const EgmFeedBack& source, const RobotAx { bool success = false; - std::cout << "EGMFeedback joints: "; - for (const auto& j : source.joints().joints()) - std::cout << j << ","; - std::cout << std::endl; - - // std::cout << "EGMFeedback external joints: "; - // for (const auto& j : source.externalJoints().joints()) - // std::cout << j << ","; - // std::cout << std::endl; - - std::cout << "EGMFeedback pose: " << source.cartesian().pos().x() << "," - << source.cartesian().pos().y() << "," - << source.cartesian().pos().z() << std::endl; - if (p_target) { success = parse(p_target->mutable_robot()->mutable_joints()->mutable_position(), diff --git a/src/egm_controller_interface.cpp b/src/egm_controller_interface.cpp index 3a832e2..b8c503e 100644 --- a/src/egm_controller_interface.cpp +++ b/src/egm_controller_interface.cpp @@ -119,16 +119,6 @@ void EGMControllerInterface::ControllerMotion::readInputs(wrapper::Input* p_inpu { boost::lock_guard lock(read_mutex_); - std::cout << "### [ControllerMotion::readInputs()] ###" << std::endl; - std::cout << "Feedback: " << std::endl; - const auto& joint_space = inputs_.feedback().robot().joints(); - const auto& cartesian_space = inputs_.feedback().robot().cartesian(); - std::cout << "Positions: ["; - for (const auto& p : joint_space.position().values()) - { - std::cout << p << ","; - } - std::cout << "]" << std::endl; p_inputs->CopyFrom(inputs_); read_data_ready_ = false; } From 2cd3a783dda412bac720743aa3c2b8d710219f49 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 28 Jul 2022 23:12:49 +0800 Subject: [PATCH 3/3] Define constants Signed-off-by: Yadunund --- include/abb_libegm/egm_common.h | 11 +++++++++++ src/egm_common.cpp | 2 ++ src/egm_common_auxiliary.cpp | 5 +++-- 3 files changed, 16 insertions(+), 2 deletions(-) diff --git a/include/abb_libegm/egm_common.h b/include/abb_libegm/egm_common.h index a9ad564..f491a50 100644 --- a/include/abb_libegm/egm_common.h +++ b/include/abb_libegm/egm_common.h @@ -100,6 +100,17 @@ struct Constants * \brief Maximum number of joints. */ static const int MAX_NUMBER_OF_JOINTS; + + /** + * \brief The index of the prismatic joint if present + */ + static const int INDEX_OF_PRISMATIC_JOINT; + + /** + * \brief The z coordinate of the end-effector when the prismatic joint is + * at its minimum value. Measured in mm. + */ + static const double Z_AXIS_OFFSET_FOR_FOUR_AXIS_ROBOT; }; /** diff --git a/src/egm_common.cpp b/src/egm_common.cpp index 13eec24..2558de3 100644 --- a/src/egm_common.cpp +++ b/src/egm_common.cpp @@ -56,6 +56,8 @@ const int RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS = 6; const int RobotController::DEFAULT_NUMBER_OF_EXTERNAL_JOINTS = 6; const int RobotController::MAX_NUMBER_OF_JOINTS = RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS + RobotController::DEFAULT_NUMBER_OF_EXTERNAL_JOINTS; +const int RobotController::INDEX_OF_PRISMATIC_JOINT = 2; +const double RobotController::Z_AXIS_OFFSET_FOR_FOUR_AXIS_ROBOT = 220.2; // ABB IRB910SC const double Constants::Conversion::RAD_TO_DEG = 180.0 / M_PI; const double Constants::Conversion::DEG_TO_RAD = M_PI / 180.0; diff --git a/src/egm_common_auxiliary.cpp b/src/egm_common_auxiliary.cpp index 774c725..d62b7ff 100644 --- a/src/egm_common_auxiliary.cpp +++ b/src/egm_common_auxiliary.cpp @@ -41,6 +41,7 @@ #include #include "abb_libegm/egm_common_auxiliary.h" +#include "abb_libegm/egm_common.h" namespace abb { @@ -889,8 +890,8 @@ bool parse(wrapper::Feedback* p_target, const EgmFeedBack& source, const RobotAx if (source.has_cartesian()) { auto mutable_values = p_target->mutable_robot()->mutable_joints()->mutable_position()->mutable_values(); - auto& val = mutable_values->at(2); - val = source.cartesian().pos().z() - 220.2; + auto& val = mutable_values->at(Constants::RobotController::INDEX_OF_PRISMATIC_JOINT); + val = source.cartesian().pos().z() - Constants::RobotController::Z_AXIS_OFFSET_FOR_FOUR_AXIS_ROBOT; } else {