Expose measured force torque data

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.
  *

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.

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_)
     {

src/egm_common_auxiliary.cpp

@@ -844,6 +844,36 @@ 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)
+  {
+    // 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;
+  }
+
+  p_target->Clear();  // No FT data
+  return true;  // No FT sensor is not an error
+}
+
+
 bool parse(wrapper::Feedback* p_target, const EgmFeedBack& source, const RobotAxes axes)
 {
   bool success = false;