From 95f0b1441371e4ae06366de81628e9815bafa73e Mon Sep 17 00:00:00 2001 From: Thomas Hettasch Date: Mon, 18 May 2026 15:00:30 +0000 Subject: [PATCH 1/2] Expose measured force torque data --- include/abb_libegm/egm_common_auxiliary.h | 10 +++++++++ proto/egm_wrapper.proto | 27 +++++++++++++++++++++++ src/egm_base_interface.cpp | 3 ++- src/egm_common_auxiliary.cpp | 25 +++++++++++++++++++++ 4 files changed, 64 insertions(+), 1 deletion(-) diff --git a/include/abb_libegm/egm_common_auxiliary.h b/include/abb_libegm/egm_common_auxiliary.h index 7cfe193..941e5c5 100644 --- a/include/abb_libegm/egm_common_auxiliary.h +++ b/include/abb_libegm/egm_common_auxiliary.h @@ -388,6 +388,16 @@ bool parse(wrapper::Joints* p_target_robot, */ bool parse(wrapper::CartesianPose* p_target, const EgmPose& source); +/** + * \brief Parse an abb::egm::EgmMeasuredForce object. + * + * \param p_target for containing the parsed data. + * \param source containing data to parse. + * + * \return bool indicating if the parsing was successful or not. + */ +bool parse(wrapper::ForceTorque* p_target, const EgmMeasuredForce& source); + /** * \brief Parse an abb::egm::EgmFeedBack object. * diff --git a/proto/egm_wrapper.proto b/proto/egm_wrapper.proto index be31f00..8c8105b 100644 --- a/proto/egm_wrapper.proto +++ b/proto/egm_wrapper.proto @@ -205,6 +205,32 @@ message Planned optional Clock time = 3; } +//====================================================================================================================== +// +// Force torque sensor measurments. +// +//====================================================================================================================== + +message Force +{ + optional double x = 1; + optional double y = 2; + optional double z = 3; +} + +message Torque +{ + optional double x = 1; + optional double y = 2; + optional double z = 3; +} + +message ForceTorque +{ + optional Force force = 1; + optional Torque torque = 2; +} + //====================================================================================================================== // // Primary messages. @@ -218,6 +244,7 @@ message Input optional Feedback feedback = 2; optional Planned planned = 3; optional Status status = 4; + optional ForceTorque forcetorque = 5; } // Outputs to send to the robot controller. diff --git a/src/egm_base_interface.cpp b/src/egm_base_interface.cpp index 0219754..6d749d5 100644 --- a/src/egm_base_interface.cpp +++ b/src/egm_base_interface.cpp @@ -90,7 +90,8 @@ bool EGMBaseInterface::InputContainer::extractParsedInformation(const RobotAxes& 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_)) + parse(current_.mutable_status(), egm_robot_) && + parse(current_.mutable_forcetorque(), egm_robot_.measuredforce())) { if (first_message_) { diff --git a/src/egm_common_auxiliary.cpp b/src/egm_common_auxiliary.cpp index d1f73b0..a82844a 100644 --- a/src/egm_common_auxiliary.cpp +++ b/src/egm_common_auxiliary.cpp @@ -844,6 +844,31 @@ bool parse(wrapper::CartesianPose* p_target, const EgmPose& source) return success; } +bool parse(wrapper::ForceTorque* p_target, const EgmMeasuredForce& source) +{ + if (!p_target) + { + return false; + } + + if (source.force_size() != 6) + { + p_target->Clear(); // No FT data + return true; // No FT sensor is not an error + } + + // Read out force torque data + p_target->mutable_force()->set_x(source.force().at(0)); + p_target->mutable_force()->set_y(source.force().at(1)); + p_target->mutable_force()->set_z(source.force().at(2)); + p_target->mutable_torque()->set_x(source.force().at(3)); + p_target->mutable_torque()->set_y(source.force().at(4)); + p_target->mutable_torque()->set_z(source.force().at(5)); + + return true; +} + + bool parse(wrapper::Feedback* p_target, const EgmFeedBack& source, const RobotAxes axes) { bool success = false; From 5057d14de443c3564ada3fe2416fbd2f6a87e64a Mon Sep 17 00:00:00 2001 From: Thomas Hettasch Date: Wed, 20 May 2026 12:19:46 +0000 Subject: [PATCH 2/2] Allow for single DOF FT sensors --- src/egm_common_auxiliary.cpp | 29 +++++++++++++++++------------ 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/src/egm_common_auxiliary.cpp b/src/egm_common_auxiliary.cpp index a82844a..e4e2456 100644 --- a/src/egm_common_auxiliary.cpp +++ b/src/egm_common_auxiliary.cpp @@ -851,21 +851,26 @@ bool parse(wrapper::ForceTorque* p_target, const EgmMeasuredForce& source) return false; } - if (source.force_size() != 6) + if (source.force_size() == 6) { - p_target->Clear(); // No FT data - return true; // No FT sensor is not an error + // Read out force torque data (6 DOF) + p_target->mutable_force()->set_x(source.force().at(0)); + p_target->mutable_force()->set_y(source.force().at(1)); + p_target->mutable_force()->set_z(source.force().at(2)); + p_target->mutable_torque()->set_x(source.force().at(3)); + p_target->mutable_torque()->set_y(source.force().at(4)); + p_target->mutable_torque()->set_z(source.force().at(5)); + return true; + } + else if (source.force_size() == 1) + { + // Read out force torque data (1 DOF) + p_target->mutable_force()->set_x(source.force().at(0)); + return true; } - // Read out force torque data - p_target->mutable_force()->set_x(source.force().at(0)); - p_target->mutable_force()->set_y(source.force().at(1)); - p_target->mutable_force()->set_z(source.force().at(2)); - p_target->mutable_torque()->set_x(source.force().at(3)); - p_target->mutable_torque()->set_y(source.force().at(4)); - p_target->mutable_torque()->set_z(source.force().at(5)); - - return true; + p_target->Clear(); // No FT data + return true; // No FT sensor is not an error }