diff --git a/include/abb_libegm/egm_common.h b/include/abb_libegm/egm_common.h index ea160a6..f491a50 100644 --- a/include/abb_libegm/egm_common.h +++ b/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; }; /** diff --git a/src/egm_base_interface.cpp b/src/egm_base_interface.cpp index 0219754..b5ca34f 100644 --- a/src/egm_base_interface.cpp +++ b/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) 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 d1f73b0..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 { @@ -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)