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)