diff --git a/include/abb_libegm/egm_common.h b/include/abb_libegm/egm_common.h index bd0e32a..4d12c2f 100644 --- a/include/abb_libegm/egm_common.h +++ b/include/abb_libegm/egm_common.h @@ -48,6 +48,7 @@ namespace egm */ enum RobotAxes { + None = 0, ///< \brief No robot axes are expected (i.e. only external axes). Six = 6, ///< \brief A six axes robot. Seven = 7 ///< \brief A seven axes robot. }; diff --git a/src/egm_base_interface.cpp b/src/egm_base_interface.cpp index 6a9a96a..6a73546 100644 --- a/src/egm_base_interface.cpp +++ b/src/egm_base_interface.cpp @@ -391,7 +391,7 @@ void EGMBaseInterface::OutputContainer::constructReply(const BaseConfiguration& constructHeader(); bool success = constructJointBody(configuration); - if (success) + if (success && configuration.axes != None) { success = constructCartesianBody(configuration); } @@ -515,6 +515,20 @@ bool EGMBaseInterface::OutputContainer::constructJointBody(const BaseConfigurati switch (configuration.axes) { + case None: + { + if (robot_position.values_size() == 0) + { + 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) diff --git a/src/egm_common_auxiliary.cpp b/src/egm_common_auxiliary.cpp index 6c933e9..d1f73b0 100644 --- a/src/egm_common_auxiliary.cpp +++ b/src/egm_common_auxiliary.cpp @@ -728,6 +728,20 @@ bool parse(wrapper::Joints* p_target_robot, switch (axes) { + case None: + { + if (source_robot.joints_size() == 0) + { + 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) @@ -842,7 +856,14 @@ bool parse(wrapper::Feedback* p_target, const EgmFeedBack& source, const RobotAx if (success) { - success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian()); + if(axes == None) + { + success = !source.has_cartesian(); + } + else + { + success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian()); + } if (success) { @@ -866,7 +887,14 @@ bool parse(wrapper::Planned* p_target, const EgmPlanned& source, const RobotAxes if (success) { - success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian()); + if(axes == None) + { + success = !source.has_cartesian(); + } + else + { + success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian()); + } if (success) { diff --git a/src/egm_trajectory_interface.cpp b/src/egm_trajectory_interface.cpp index 6d25004..84013cf 100644 --- a/src/egm_trajectory_interface.cpp +++ b/src/egm_trajectory_interface.cpp @@ -194,7 +194,7 @@ wrapper::trajectory::ExecutionProgress_SubState EGMTrajectoryInterface::Trajecto void EGMTrajectoryInterface::TrajectoryMotion::MotionStep::resetMotionStep() { unsigned int robot_joints = data.feedback.robot().joints().position().values_size(); - unsigned int external_joints = data.feedback.robot().joints().position().values_size(); + unsigned int external_joints = data.feedback.external().joints().position().values_size(); internal_goal.Clear(); external_goal.Clear(); @@ -243,7 +243,7 @@ void EGMTrajectoryInterface::TrajectoryMotion::MotionStep::resetMotionStep() void EGMTrajectoryInterface::TrajectoryMotion::MotionStep::prepareNormalGoal(const bool last_point) { unsigned int robot_joints = data.feedback.robot().joints().position().values_size(); - unsigned int external_joints = data.feedback.robot().joints().position().values_size(); + unsigned int external_joints = data.feedback.external().joints().position().values_size(); data.mode = (external_goal.robot().has_cartesian() ? EGMPose : EGMJoint); @@ -394,7 +394,7 @@ double EGMTrajectoryInterface::TrajectoryMotion::MotionStep::estimateDuration() double estimate = 0.0; unsigned int robot_joints = data.feedback.robot().joints().position().values_size(); - unsigned int external_joints = data.feedback.robot().joints().position().values_size(); + unsigned int external_joints = data.feedback.external().joints().position().values_size(); // Reset robot joint values. reset(internal_goal.mutable_robot()->mutable_joints()->mutable_velocity(), robot_joints);