ros-industrial / abb_libegm

A C++ library for interfacing with ABB robot controllers supporting Externally Guided Motion (689-1)
BSD 3-Clause "New" or "Revised" License
93 stars 53 forks source link

EGM Move not allowed in External Motion Interface #110

Closed Luc90 closed 3 years ago

Luc90 commented 3 years ago

Hello,

I use the ABB IRB 1200-5/0.9, RobotWare 6.11.01 and I work with the real robot.

I try to use EGM with the code provided in #18 Create code samples. I used the code "a1_joint_trajectory_node.cpp" successfully but when I try to use the code "a2_pose_trajectory_node.cpp", I get the following error on the Flexpendant "Event Message 50453", "Move not allowed in External Motion Interface".

image

When I run the Rapid code no error appears but when I run the "a2_pose_trjectory_node.cpp" code the Rapid code stop with the error mentioned above.

The Rapid code in the controller is the code provided in "EGMSamplesSolution Pack&Go".

`

!***********************************************************
! Program data
!***********************************************************
! Home position.
LOCAL CONST jointtarget home := [[0, 0, 0, 0, 30, 0], [9E9, 9E9, 9E9, 9E9, 9E9, 9E9]];

! Identifier for the EGM correction.
LOCAL VAR egmident egm_id;

! Limits for convergance.
LOCAL VAR egm_minmax egm_condition := [-0.1, 0.1];

! EGM pose frames.
LOCAL CONST pose egm_correction_frame := [[0, 0, 0], [1, 0, 0, 0]];
LOCAL CONST pose egm_sensor_frame     := [[0, 0, 0], [1, 0, 0, 0]];

! The work object (here set to coincidence with ROB_2's base frame).
LOCAL PERS wobjdata egm_wobj := [FALSE, TRUE, "", [[0, 1000, 0],[1, 0, 0, 0]], [[0, 0, 0],[1, 0, 0, 0]]];

!***********************************************************
!
! Procedure main
!
!   This RAPID code exemplify how to run EGM pose motions.
!
!   Note: Update the UCDevice "ROB_2" with correct
!         values for the remote address and port
!         (i.e. to the EGM server).
!
!         Update via RobotStudio:
!         Controller tab -> Configuration ->
!         Communication -> Transmission Protocol
!
!***********************************************************
PROC main()
    WHILE TRUE DO
        MoveAbsJ home, v200, fine, tool0;

        ! Register an EGM id.
        EGMGetId egm_id;

        ! Setup the EGM communication.
        EGMSetupUC ROB_2, egm_id, "default", "ROB_2", \Pose;

        ! Prepare for an EGM communication session.
        EGMActPose egm_id,
                   \WObj:=egm_wobj,
                   egm_correction_frame,
                   EGM_FRAME_BASE,
                   egm_sensor_frame,
                   EGM_FRAME_BASE
                   \X:=egm_condition
                   \Y:=egm_condition
                   \Z:=egm_condition
                   \Rx:=egm_condition
                   \Ry:=egm_condition
                   \Rz:=egm_condition
                   \MaxSpeedDeviation:=20.0;

        ! Start the EGM communication session.
        EGMRunPose egm_id, EGM_STOP_RAMP_DOWN, \X \Y \Z \Rx \Ry \Rz \CondTime:=5 \RampOutTime:=5;

        ! Release the EGM id.
        EGMReset egm_id;

        WaitTime 5;
    ENDWHILE

ERROR
    IF ERRNO = ERR_UDPUC_COMM THEN
        TPWrite "Communication timedout";
        TRYNEXT;
    ENDIF
ENDPROC

`

I tried to get out EGMGetId egm_id; & EGMReset egm_id of the loop as mentioned in #54 but the error is still present. Do you know where this error comes from ?

Thanks for your help.

jontje commented 3 years ago

Hi @Luc90,

Can you check what state the EGM channel is in before and after the EGM RAPID instructions?

I.e. use EGMGetState(egm_id), which should be in any of these states:

Otherwise, you could try and use the RobotWare StateMachine Add-In. It is more stable than the RAPID example codes in https://github.com/ros-industrial/abb_libegm/issues/18.

gavanderhoorn commented 3 years ago

Actually, I'm going to close this for now, due to inactivity.

If this is a still a problem for you @Luc90, please comment and we'll re-open.