From cac70cfa41adee8aedaa3dc56231467af4d20bb9 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 28 Jul 2022 16:30:57 +0800 Subject: [PATCH 1/5] Successfully able to connect to EGM client on robot Signed-off-by: Yadunund --- include/abb_libegm/egm_common.h | 1 + src/egm_base_interface.cpp | 90 ++++++++++++++++++++++++++++++-- src/egm_common_auxiliary.cpp | 22 ++++++++ src/egm_controller_interface.cpp | 10 ++++ 4 files changed, 120 insertions(+), 3 deletions(-) diff --git a/include/abb_libegm/egm_common.h b/include/abb_libegm/egm_common.h index ea160a6..a9ad564 100644 --- a/include/abb_libegm/egm_common.h +++ b/include/abb_libegm/egm_common.h @@ -52,6 +52,7 @@ namespace egm enum RobotAxes { None = 0, ///< \brief No robot axes are expected (i.e. only external axes). + Four = 4, ///< \brief A four axes robot. Six = 6, ///< \brief A six axes robot. Seven = 7 ///< \brief A seven axes robot. }; diff --git a/src/egm_base_interface.cpp b/src/egm_base_interface.cpp index 0219754..c462d3f 100644 --- a/src/egm_base_interface.cpp +++ b/src/egm_base_interface.cpp @@ -42,6 +42,8 @@ #include "abb_libegm/egm_base_interface.h" #include "abb_libegm/egm_common_auxiliary.h" +#include + namespace abb { namespace egm @@ -84,26 +86,45 @@ bool EGMBaseInterface::InputContainer::extractParsedInformation(const RobotAxes& { bool success = false; + std::cout << " [extractParsedInformation] Input RobotAxes: " << axes << std::endl; detectRWAndEGMVersions(); + bool ok; + std::cout << " [extractParsedInformation] has_new_data: " << has_new_data_ << std::endl; + ok = parse(current_.mutable_header(), egm_robot_.header()); + std::cout << "Ok after parsing mutable_header: " << ok << std::endl; + ok = parse(current_.mutable_feedback(), egm_robot_.feedback(), axes); + std::cout << "Ok after parsing mutable_feedback: " << ok << std::endl; + ok = parse(current_.mutable_planned(), egm_robot_.planned(), axes); + std::cout << "Ok after parsing mutable_planned: " << ok << std::endl; + ok = parse(current_.mutable_status(), egm_robot_); + std::cout << "Ok after parsing mutable_status: " << ok << std::endl; + if (has_new_data_ && parse(current_.mutable_header(), egm_robot_.header()) && parse(current_.mutable_feedback(), egm_robot_.feedback(), axes) && parse(current_.mutable_planned(), egm_robot_.planned(), axes) && parse(current_.mutable_status(), egm_robot_)) { + std::cout << " [extractParsedInformation] has_new_data and all parse true " << std::endl; if (first_message_) { + std::cout << " [extractParsedInformation] first_message_ is true: " << std::endl; initial_.CopyFrom(current_); previous_.CopyFrom(current_); } estimated_sample_time_ = estimateSampleTime(); + std::cout << " [extractParsedInformation] success before estimateAllVelocities(): " + << success << std::endl; success = estimateAllVelocities(); - + std::cout << " [extractParsedInformation] success after estimateAllVelocities(): " + << success << std::endl; has_new_data_ = false; } + std::cout << " [extractParsedInformation] success before returning: " + << success << std::endl; return success; } @@ -136,6 +157,7 @@ void EGMBaseInterface::InputContainer::detectRWAndEGMVersions() { if(has_new_data_) { + std::cout << "[detectRWAndEGMVersions] has_new_data" << std::endl; // Time field was added in RobotWare '6.07', as well as fix of inconsistent units (e.g. radians and degrees). if(egm_robot_.feedback().has_time()) { @@ -169,9 +191,13 @@ void EGMBaseInterface::InputContainer::detectRWAndEGMVersions() } else { + std::cout << "[detectRWAndEGMVersions] No new data. Setting RW & EGM version to unknown" << std::endl; current_.mutable_header()->set_rw_version(wrapper::Header_RWVersion_RW_UNKNOWN); current_.mutable_header()->set_egm_version(wrapper::Header_EGMVersion_EGM_UNKNOWN); } + std::cout << "[detectRWAndEGMVersions] set_rw_version to: " << current_.header().rw_version() << std::endl; + std::cout << "[detectRWAndEGMVersions] set_egm_version to: " << current_.header().egm_version() << std::endl; + } double EGMBaseInterface::InputContainer::estimateSampleTime() @@ -399,19 +425,28 @@ void EGMBaseInterface::OutputContainer::constructReply(const BaseConfiguration& { constructHeader(); bool success = constructJointBody(configuration); + std::cout << "[EGMBaseInterface::constructReply()] success after constructJointBody(): " + << success << std::endl; if (success && configuration.axes != None) { + std::cout << "constructing cartesian body" << std::endl; success = constructCartesianBody(configuration); + std::cout << "[EGMBaseInterface::constructReply()] success after constructCartesianBody(): " + << success << std::endl; } if (success) { + std::cout << "Serializing to string" << std::endl; success = egm_sensor_.SerializeToString(&reply_); + std::cout << "[EGMBaseInterface::constructReply()] success after SerializeToString(): " + << success << std::endl; } if (!success) { + std::cout << " success is false CLEARING REPLY" << std::endl; reply_.clear(); } } @@ -504,7 +539,8 @@ bool EGMBaseInterface::OutputContainer::constructJointBody(const BaseConfigurati int rob_condition = Constants::RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS; int ext_condition = Constants::RobotController::DEFAULT_NUMBER_OF_EXTERNAL_JOINTS; - + std::cout << "[constructJointBody] rob_condition: " << rob_condition << std::endl; + std::cout << "[constructJointBody] ext_condition: " << ext_condition << std::endl; if (current.robot().joints().has_position()) { // Outputs. @@ -538,6 +574,26 @@ bool EGMBaseInterface::OutputContainer::constructJointBody(const BaseConfigurati } break; + case Four: + { + // If using a four axes robot (e.g. IRB910SC)): Map to special case. + if (robot_position.values_size() == rob_condition - 2) + { + for (int i = 0; i < robot_position.values_size(); ++i) + { + planned->mutable_joints()->add_joints(robot_position.values(i)); + } + + for (int i = 0; i < external_position.values_size() && i < ext_condition; ++i) + { + planned->mutable_externaljoints()->add_joints(external_position.values(i)); + } + + position_ok = true; + } + } + break; + case Six: { if (robot_position.values_size() == rob_condition) @@ -615,6 +671,27 @@ bool EGMBaseInterface::OutputContainer::constructJointBody(const BaseConfigurati } break; + // Same as Six + case Four: + { + // If using a four axes robot (e.g. IRB910SC): Map to special case. + if (robot_velocity.values_size() == rob_condition - 2) + { + for (int i = 0; i < robot_velocity.values_size(); ++i) + { + speed_reference->mutable_joints()->add_joints(robot_velocity.values(i)); + } + + for (int i = 0; i < external_velocity.values_size() && i < ext_condition; ++i) + { + speed_reference->mutable_externaljoints()->add_joints(external_velocity.values(i)); + } + + speed_ok = true; + } + } + break; + case Six: { if (robot_velocity.values_size() == rob_condition) @@ -860,12 +937,15 @@ bool EGMBaseInterface::initializeCallback(const UDPServerData& server_data) // Parse the received message. if (server_data.p_data) { + std::cout << "server_data.p_data is true. calling parseFromArray()" < lock(configuration_.mutex); if (configuration_.has_pending_update) @@ -878,8 +958,9 @@ bool EGMBaseInterface::initializeCallback(const UDPServerData& server_data) // Extract information from the parsed message. if (success) { + std::cout << "success. Calling extractParsedInformation()..." < lock(session_data_.mutex); @@ -891,6 +972,7 @@ bool EGMBaseInterface::initializeCallback(const UDPServerData& server_data) } else { + std::cout << "Calling clear" << std::endl; session_data_.header.Clear(); session_data_.status.Clear(); } @@ -901,7 +983,9 @@ bool EGMBaseInterface::initializeCallback(const UDPServerData& server_data) outputs_.clearReply(); if (success) { + std::cout << "success. Calling prepareOutputs()..." < + namespace abb { namespace egm @@ -723,6 +725,8 @@ bool parse(wrapper::Joints* p_target_robot, if (p_target_robot && p_target_external) { + std::cout << " [egm_common_auxiliary::parse()] source_robot.joints_size: " + << source_robot.joints_size() << std::endl; p_target_robot->Clear(); p_target_external->Clear(); @@ -742,6 +746,24 @@ bool parse(wrapper::Joints* p_target_robot, } break; + case Four: + { + if (source_robot.joints_size() == 4) + { + for (int i = 0; i < source_robot.joints_size(); ++i) + { + p_target_robot->add_values(source_robot.joints(i)); + } + + for (int i = 0; i < source_external.joints_size(); ++i) + { + p_target_external->add_values(source_external.joints(i)); + } + + success = true; + } + } + break; case Six: { if (source_robot.joints_size() == Constants::RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS) diff --git a/src/egm_controller_interface.cpp b/src/egm_controller_interface.cpp index b8c503e..4b566fd 100644 --- a/src/egm_controller_interface.cpp +++ b/src/egm_controller_interface.cpp @@ -160,33 +160,41 @@ EGMBaseInterface(io_service, port_number, configuration) const std::string& EGMControllerInterface::callback(const UDPServerData& server_data) { + std::cout << "[EGMControllerInterface::callback()] initializing Callback..." << std::endl; + // Initialize the callback by: // - Parsing and extracting data from the received message. // - Updating any pending configuration changes. // - Preparing the outputs. if (initializeCallback(server_data)) { + std::cout << "[EGMControllerInterface::callback()] initializing controller_motion..." << std::endl; + // Additional initialization for direct motion references. controller_motion_.initialize(inputs_.isFirstMessage()); // Handle demo execution or external controller execution. if (configuration_.active.use_demo_outputs) { + std::cout << "[EGMControllerInterface::callback()] use_demo_outputs enabled" << std::endl; outputs_.generateDemoOutputs(inputs_); } else { + std::cout << "[EGMControllerInterface::callback()] use_demo_outputs not enabled" << std::endl; // Make the current inputs available (to the external control loop), and notify that it is available. controller_motion_.writeInputs(inputs_.current()); // Send a notification via the (optional) external condition variable. if(configuration_.active.p_new_message_cv) { + std::cout << "[EGMControllerInterface::callback()] notifying all" << std::endl; configuration_.active.p_new_message_cv->notify_all(); } if (inputs_.isFirstMessage() || inputs_.statesOk()) { + std::cout << "[EGMControllerInterface::callback()] isFirstMessage" << std::endl; // Wait for new outputs (from the external control loop), or until a timeout occurs. controller_motion_.readOutputs(&outputs_.current); } @@ -198,6 +206,7 @@ const std::string& EGMControllerInterface::callback(const UDPServerData& server_ logData(inputs_, outputs_, configuration_.active.max_logging_duration); } + std::cout << "[EGMControllerInterface::callback()] Constructing reply..." << std::endl; // Constuct the reply message. outputs_.constructReply(configuration_.active); @@ -206,6 +215,7 @@ const std::string& EGMControllerInterface::callback(const UDPServerData& server_ outputs_.updatePrevious(); } + std::cout << "[EGMControllerInterface::callback()] replying..." << std::endl; // Return the reply. return outputs_.reply(); } From 67492ff63c0b1d0f52ea12652fe19935282c4c3a Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 28 Jul 2022 18:44:48 +0800 Subject: [PATCH 2/5] Cleanup printouts Signed-off-by: Yadunund --- src/egm_base_interface.cpp | 49 +------------------------------- src/egm_common_auxiliary.cpp | 4 --- src/egm_controller_interface.cpp | 11 ------- 3 files changed, 1 insertion(+), 63 deletions(-) diff --git a/src/egm_base_interface.cpp b/src/egm_base_interface.cpp index c462d3f..127085b 100644 --- a/src/egm_base_interface.cpp +++ b/src/egm_base_interface.cpp @@ -42,8 +42,6 @@ #include "abb_libegm/egm_base_interface.h" #include "abb_libegm/egm_common_auxiliary.h" -#include - namespace abb { namespace egm @@ -85,46 +83,25 @@ bool EGMBaseInterface::InputContainer::parseFromArray(const char* data, const in bool EGMBaseInterface::InputContainer::extractParsedInformation(const RobotAxes& axes) { bool success = false; - - std::cout << " [extractParsedInformation] Input RobotAxes: " << axes << std::endl; detectRWAndEGMVersions(); - bool ok; - std::cout << " [extractParsedInformation] has_new_data: " << has_new_data_ << std::endl; - ok = parse(current_.mutable_header(), egm_robot_.header()); - std::cout << "Ok after parsing mutable_header: " << ok << std::endl; - ok = parse(current_.mutable_feedback(), egm_robot_.feedback(), axes); - std::cout << "Ok after parsing mutable_feedback: " << ok << std::endl; - ok = parse(current_.mutable_planned(), egm_robot_.planned(), axes); - std::cout << "Ok after parsing mutable_planned: " << ok << std::endl; - ok = parse(current_.mutable_status(), egm_robot_); - std::cout << "Ok after parsing mutable_status: " << ok << std::endl; - if (has_new_data_ && parse(current_.mutable_header(), egm_robot_.header()) && parse(current_.mutable_feedback(), egm_robot_.feedback(), axes) && parse(current_.mutable_planned(), egm_robot_.planned(), axes) && parse(current_.mutable_status(), egm_robot_)) { - std::cout << " [extractParsedInformation] has_new_data and all parse true " << std::endl; if (first_message_) { - std::cout << " [extractParsedInformation] first_message_ is true: " << std::endl; initial_.CopyFrom(current_); previous_.CopyFrom(current_); } estimated_sample_time_ = estimateSampleTime(); - std::cout << " [extractParsedInformation] success before estimateAllVelocities(): " - << success << std::endl; success = estimateAllVelocities(); - std::cout << " [extractParsedInformation] success after estimateAllVelocities(): " - << success << std::endl; has_new_data_ = false; } - std::cout << " [extractParsedInformation] success before returning: " - << success << std::endl; return success; } @@ -157,7 +134,6 @@ void EGMBaseInterface::InputContainer::detectRWAndEGMVersions() { if(has_new_data_) { - std::cout << "[detectRWAndEGMVersions] has_new_data" << std::endl; // Time field was added in RobotWare '6.07', as well as fix of inconsistent units (e.g. radians and degrees). if(egm_robot_.feedback().has_time()) { @@ -191,13 +167,9 @@ void EGMBaseInterface::InputContainer::detectRWAndEGMVersions() } else { - std::cout << "[detectRWAndEGMVersions] No new data. Setting RW & EGM version to unknown" << std::endl; current_.mutable_header()->set_rw_version(wrapper::Header_RWVersion_RW_UNKNOWN); current_.mutable_header()->set_egm_version(wrapper::Header_EGMVersion_EGM_UNKNOWN); } - std::cout << "[detectRWAndEGMVersions] set_rw_version to: " << current_.header().rw_version() << std::endl; - std::cout << "[detectRWAndEGMVersions] set_egm_version to: " << current_.header().egm_version() << std::endl; - } double EGMBaseInterface::InputContainer::estimateSampleTime() @@ -425,28 +397,18 @@ void EGMBaseInterface::OutputContainer::constructReply(const BaseConfiguration& { constructHeader(); bool success = constructJointBody(configuration); - std::cout << "[EGMBaseInterface::constructReply()] success after constructJointBody(): " - << success << std::endl; - if (success && configuration.axes != None) { - std::cout << "constructing cartesian body" << std::endl; success = constructCartesianBody(configuration); - std::cout << "[EGMBaseInterface::constructReply()] success after constructCartesianBody(): " - << success << std::endl; } if (success) { - std::cout << "Serializing to string" << std::endl; success = egm_sensor_.SerializeToString(&reply_); - std::cout << "[EGMBaseInterface::constructReply()] success after SerializeToString(): " - << success << std::endl; } if (!success) { - std::cout << " success is false CLEARING REPLY" << std::endl; reply_.clear(); } } @@ -539,8 +501,7 @@ bool EGMBaseInterface::OutputContainer::constructJointBody(const BaseConfigurati int rob_condition = Constants::RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS; int ext_condition = Constants::RobotController::DEFAULT_NUMBER_OF_EXTERNAL_JOINTS; - std::cout << "[constructJointBody] rob_condition: " << rob_condition << std::endl; - std::cout << "[constructJointBody] ext_condition: " << ext_condition << std::endl; + if (current.robot().joints().has_position()) { // Outputs. @@ -937,15 +898,12 @@ bool EGMBaseInterface::initializeCallback(const UDPServerData& server_data) // Parse the received message. if (server_data.p_data) { - std::cout << "server_data.p_data is true. calling parseFromArray()" < lock(configuration_.mutex); if (configuration_.has_pending_update) @@ -958,9 +916,7 @@ bool EGMBaseInterface::initializeCallback(const UDPServerData& server_data) // Extract information from the parsed message. if (success) { - std::cout << "success. Calling extractParsedInformation()..." < lock(session_data_.mutex); @@ -972,7 +928,6 @@ bool EGMBaseInterface::initializeCallback(const UDPServerData& server_data) } else { - std::cout << "Calling clear" << std::endl; session_data_.header.Clear(); session_data_.status.Clear(); } @@ -983,9 +938,7 @@ bool EGMBaseInterface::initializeCallback(const UDPServerData& server_data) outputs_.clearReply(); if (success) { - std::cout << "success. Calling prepareOutputs()..." < - namespace abb { namespace egm @@ -725,8 +723,6 @@ bool parse(wrapper::Joints* p_target_robot, if (p_target_robot && p_target_external) { - std::cout << " [egm_common_auxiliary::parse()] source_robot.joints_size: " - << source_robot.joints_size() << std::endl; p_target_robot->Clear(); p_target_external->Clear(); diff --git a/src/egm_controller_interface.cpp b/src/egm_controller_interface.cpp index 4b566fd..639bd35 100644 --- a/src/egm_controller_interface.cpp +++ b/src/egm_controller_interface.cpp @@ -128,7 +128,6 @@ void EGMControllerInterface::ControllerMotion::writeOutputs(const wrapper::Outpu boost::lock_guard lock(write_mutex_); outputs_.CopyFrom(outputs); - write_data_ready_ = true; write_condition_variable_.notify_all(); } @@ -160,41 +159,33 @@ EGMBaseInterface(io_service, port_number, configuration) const std::string& EGMControllerInterface::callback(const UDPServerData& server_data) { - std::cout << "[EGMControllerInterface::callback()] initializing Callback..." << std::endl; - // Initialize the callback by: // - Parsing and extracting data from the received message. // - Updating any pending configuration changes. // - Preparing the outputs. if (initializeCallback(server_data)) { - std::cout << "[EGMControllerInterface::callback()] initializing controller_motion..." << std::endl; - // Additional initialization for direct motion references. controller_motion_.initialize(inputs_.isFirstMessage()); // Handle demo execution or external controller execution. if (configuration_.active.use_demo_outputs) { - std::cout << "[EGMControllerInterface::callback()] use_demo_outputs enabled" << std::endl; outputs_.generateDemoOutputs(inputs_); } else { - std::cout << "[EGMControllerInterface::callback()] use_demo_outputs not enabled" << std::endl; // Make the current inputs available (to the external control loop), and notify that it is available. controller_motion_.writeInputs(inputs_.current()); // Send a notification via the (optional) external condition variable. if(configuration_.active.p_new_message_cv) { - std::cout << "[EGMControllerInterface::callback()] notifying all" << std::endl; configuration_.active.p_new_message_cv->notify_all(); } if (inputs_.isFirstMessage() || inputs_.statesOk()) { - std::cout << "[EGMControllerInterface::callback()] isFirstMessage" << std::endl; // Wait for new outputs (from the external control loop), or until a timeout occurs. controller_motion_.readOutputs(&outputs_.current); } @@ -206,7 +197,6 @@ const std::string& EGMControllerInterface::callback(const UDPServerData& server_ logData(inputs_, outputs_, configuration_.active.max_logging_duration); } - std::cout << "[EGMControllerInterface::callback()] Constructing reply..." << std::endl; // Constuct the reply message. outputs_.constructReply(configuration_.active); @@ -215,7 +205,6 @@ const std::string& EGMControllerInterface::callback(const UDPServerData& server_ outputs_.updatePrevious(); } - std::cout << "[EGMControllerInterface::callback()] replying..." << std::endl; // Return the reply. return outputs_.reply(); } From 2453504cc0061b71e96379061442c56964024b6c Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 28 Jul 2022 18:49:42 +0800 Subject: [PATCH 3/5] Undo line breaks Signed-off-by: Yadunund --- src/egm_base_interface.cpp | 4 ++++ src/egm_controller_interface.cpp | 1 + 2 files changed, 5 insertions(+) diff --git a/src/egm_base_interface.cpp b/src/egm_base_interface.cpp index 127085b..c3f6a48 100644 --- a/src/egm_base_interface.cpp +++ b/src/egm_base_interface.cpp @@ -83,6 +83,7 @@ bool EGMBaseInterface::InputContainer::parseFromArray(const char* data, const in bool EGMBaseInterface::InputContainer::extractParsedInformation(const RobotAxes& axes) { bool success = false; + detectRWAndEGMVersions(); if (has_new_data_ && @@ -99,6 +100,7 @@ bool EGMBaseInterface::InputContainer::extractParsedInformation(const RobotAxes& estimated_sample_time_ = estimateSampleTime(); success = estimateAllVelocities(); + has_new_data_ = false; } @@ -397,6 +399,7 @@ void EGMBaseInterface::OutputContainer::constructReply(const BaseConfiguration& { constructHeader(); bool success = constructJointBody(configuration); + if (success && configuration.axes != None) { success = constructCartesianBody(configuration); @@ -917,6 +920,7 @@ bool EGMBaseInterface::initializeCallback(const UDPServerData& server_data) if (success) { success = inputs_.extractParsedInformation(configuration_.active.axes); + { boost::lock_guard lock(session_data_.mutex); diff --git a/src/egm_controller_interface.cpp b/src/egm_controller_interface.cpp index 639bd35..b8c503e 100644 --- a/src/egm_controller_interface.cpp +++ b/src/egm_controller_interface.cpp @@ -128,6 +128,7 @@ void EGMControllerInterface::ControllerMotion::writeOutputs(const wrapper::Outpu boost::lock_guard lock(write_mutex_); outputs_.CopyFrom(outputs); + write_data_ready_ = true; write_condition_variable_.notify_all(); } From fd40b7ad0f9a7203c49a343662e3a83f674c5c58 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 28 Jul 2022 19:17:29 +0800 Subject: [PATCH 4/5] Remove comment Signed-off-by: Yadunund --- src/egm_base_interface.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/egm_base_interface.cpp b/src/egm_base_interface.cpp index c3f6a48..b5ca34f 100644 --- a/src/egm_base_interface.cpp +++ b/src/egm_base_interface.cpp @@ -635,7 +635,6 @@ bool EGMBaseInterface::OutputContainer::constructJointBody(const BaseConfigurati } break; - // Same as Six case Four: { // If using a four axes robot (e.g. IRB910SC): Map to special case. From 60bcc398291de944057cd519c7c0be480d823065 Mon Sep 17 00:00:00 2001 From: Yadu Date: Thu, 28 Jul 2022 23:33:44 +0800 Subject: [PATCH 5/5] Fix joint value reported for prismatic axis (#1) * Estimate linear joint value from cartesian pose Signed-off-by: Yadunund * Cleanup Signed-off-by: Yadunund * Define constants Signed-off-by: Yadunund --- include/abb_libegm/egm_common.h | 11 +++++++++++ src/egm_common.cpp | 2 ++ src/egm_common_auxiliary.cpp | 17 +++++++++++++++++ 3 files changed, 30 insertions(+) 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 5316b04..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 { @@ -881,6 +882,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(Constants::RobotController::INDEX_OF_PRISMATIC_JOINT); + val = source.cartesian().pos().z() - Constants::RobotController::Z_AXIS_OFFSET_FOR_FOUR_AXIS_ROBOT; + } + else + { + success = false; + } + } } if (success)