use EGM to command joint positions while getting wrench from FCGetForce

buckleytoby

RW 7.8

ABB Gofa

I want to control joint states using EGM using a client pc. At the same time I want to use EGM's /DataToSensor function to send eef wrench's (obtained using FCGetForce) to the EGM client.

This is the code I'm trying to run:

PROC run_egm_joint_server()

    FCDeact;

    FCCalib load_mount_plate;

   

    ! Register an EGM id.

    EGMGetId egm_id;

    egmSt1:=EGMGetState(egm_id);

    TPWrite "EGM state: "\Num:=egmSt1;

   

    ! setup

    IF egmSt1 >= EGM_STATE_RUNNING then

        EGMStop egm_id, EGM_STOP_HOLD;

    ENDIF

   

    ! MoveAbsJ start_j, v50, fine, tool0;

    MoveJ start_p, v30, fine, tool_mount_plate; ! tool_shadow

   

   

    IF egmSt1 <= EGM_STATE_CONNECTED THEN

        ! Setup the EGM communication. -> UC uses UDPUC

        EGMSetupUC ROB_1, egm_id, "default", "ROB_1", \Joint;

        EGMStreamStart egm_id;

    ENDIF

   

   

    ! Prepare for an EGM communication session.

    EGMActJoint egm_id

                \DataToSensor:=wrench

                \J1:=egm_condition

                \J2:=egm_condition

                \J3:=egm_condition

                \J4:=egm_condition

                \J5:=egm_condition

                \J6:=egm_condition

                \MaxSpeedDeviation:=20.0; ! degrees/s

   

    WHILE egmSt1 >= EGM_STATE_CONNECTED DO

        IF NOT egmSt1 = EGM_STATE_RUNNING THEN

            ! Start the EGM communication session.

            EGMRunJoint egm_id, EGM_STOP_HOLD, \NoWaitCond, \J1 \J2 \J3 \J4 \J5 \J6;

        ENDIF

       

        ! force mode stuff

        IF NOT FCIsForceMode() THEN

            FCAct tool_mount_plate; !Activates Force Control – This activates Force Feedback in EGM

        ENDIF

        myForceVector := FCGetForce();

        fill_wrench(myForceVector);

       

        ! wait to complete egm movement

        ! EGMWaitCond egm_id;

       

        ! check for connection

        egmSt1:=EGMGetState(egm_id);

        WaitTime 0.004;

    ENDWHILE

   

    IF egmSt1 >= EGM_STATE_CONNECTED THEN

        TPWrite "Reset EGM instance egm_id";

        EGMStop egm_id, EGM_STOP_HOLD;

        EGMStreamStop egm_id;

        EGMReset egm_id;

    ENDIF

       

    FCDeact;

ERROR

    IF ERRNO = ERR_UDPUC_COMM THEN

        TPWrite "Communication timedout";

        TRYNEXT;

    ENDIF

ENDPROC

I can successfully get the real-time wrench, but no EGM joint states that I write to 'output' and send to the Gofa arm get executed.