Fix joint value reported for prismatic axis (#1)

* Estimate linear joint value from cartesian pose Signed-off-by: Yadunund <[email protected]> * Cleanup Signed-off-by: Yadunund <[email protected]> * Define constants Signed-off-by: Yadunund <[email protected]>
Yadunund · Jul 28, 2022 · 60bcc39 · 60bcc39
1 parent fd40b7a
commit 60bcc39
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 0 deletions.
diff --git 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
@@ -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
@@ -41,6 +41,7 @@
 #include <boost/math/quaternion.hpp>
 
 #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)