Support Four Axes TCP Robots

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.
 };
@@ -99,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;
   };
 
   /**

src/egm_base_interface.cpp

@@ -538,6 +538,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 +635,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_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)

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;

src/egm_common_auxiliary.cpp

@@ -41,6 +41,7 @@
 #include <boost/math/quaternion.hpp>
 
 #include "abb_libegm/egm_common_auxiliary.h"
+#include "abb_libegm/egm_common.h"
 
 namespace abb
 {
@@ -742,6 +743,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)
@@ -863,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)