# use EGM to command joint positions while getting wrench from FCGetForce

**URL:** <https://tech-community.robotics.abb.com/t/use-egm-to-command-joint-positions-while-getting-wrench-from-fcgetforce/11150>\
**Category:** RAPID Programming\
**Created:** [June 15, 2023, 5:48am UTC](https://tech-community.robotics.abb.com/t/use-egm-to-command-joint-positions-while-getting-wrench-from-fcgetforce/11150 "2023-06-15T05:48:43Z")\
**Posts on this page:** 1\
**Page:** 1

<div class="post-metadata">

**Author:** ![buckleytoby](https://avatars.discourse-cdn.com/v4/letter/b/e0b2c6/32.png) [@buckleytoby](https://tech-community.robotics.abb.com/u/buckleytoby)\
**Post date:** [June 15, 2023, 5:48am UTC](https://tech-community.robotics.abb.com/t/use-egm-to-command-joint-positions-while-getting-wrench-from-fcgetforce/11150/1 "2023-06-15T05:48:43Z")

</div>

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.