Reading Force Vector - GoFa - RAPID

compas_rrc
1

I am attempting to read the force data from a GoFa CRB15000. For now, I am trying to write a custom instruction, to send an instruction from grasshopper in order to read such force vector, but I don’t know how to properly code in RAPID. Any help on this ? How can I transfer/print the data of the vector as the output of this custom instruction?

I defined myForceVector at the end of T_ROB1/RRC_Base_DataTask_Rob with VAR fcforcevector myForceVector;

    PROC r_RRC_read_Force()

        ! Get force vector
        myForceVector:=FCGetForce(\ContactForce);

        !
        ! Feedback
        IF bm_RRC_RecBufferRob{n_RRC_ChaNr,n_RRC_ReadPtrRecBuf}.Data.F_Lev>0 THEN
            !
            ! Cenerate feedback
            TEST bm_RRC_RecBufferRob{n_RRC_ChaNr,n_RRC_ReadPtrRecBuf}.Data.F_Lev
            CASE 1:
                !
                ! Instruction done 
                r_RRC_FDone;
            DEFAULT:
                !
                ! Feedback not supported  
                r_RRC_FError;
            ENDTEST
            !
            ! Move message in send buffer
            r_RRC_MovMsgToSenBufRob n_RRC_ChaNr;
        ENDIF
        RETURN ;
    ERROR
        ! Placeholder for Error Code...
    ENDPROC

Thanks in advance

2

I can’t get the feedback in grasshopper. Still not working but now I have this.

    PROC r_RRC_read_Force()

        ! Get force vector
        myForceVector:=FCGetForce(\ContactForce);

        ! Read input value and add it to feedback
        r_RRC_FAddValue(7);
        !myForceVector.xforce
        
        TPWrite("xforce"+ValToStr(myForceVector.xforce));
        TPWrite("value"+ValToStr(bm_RRC_ActSenMsgRob.Data.V1));
        TPWrite("counter"+ValToStr(bm_RRC_ActSenMsgRob.Data.V_Cnt));
    
        ! Feedback
        IF bm_RRC_RecBufferRob{n_RRC_ChaNr,n_RRC_ReadPtrRecBuf}.Data.F_Lev>0 THEN
            !
            ! Cenerate feedback
            TEST bm_RRC_RecBufferRob{n_RRC_ChaNr,n_RRC_ReadPtrRecBuf}.Data.F_Lev
            CASE 1:
                !
                ! Instruction done 
                r_RRC_FDone;
            DEFAULT:
                !
                ! Feedback not supported  
                r_RRC_FError;
            ENDTEST
            !
            ! Move message in send buffer
            r_RRC_MovMsgToSenBufRob n_RRC_ChaNr;
        ENDIF
        RETURN ;
3

We narrowed down the problem to the CustomInstruction that is not giving any feedback, including when specifying feedbacklevel=1 on the python side of compas_RRC (in the grasshopper component).
We tried to add a parser to the CustomInstruction source code in opt/anaconda3/envs/fs2022/lib/python3.8/site-packages/compas_rrc/custom.py but it did not solve the issue.
We are working on mac, with the M1 Pro chip, although I doubt this makes any difference.
Any help ?

4

On the RAPID side you have to add the following code to your custom instruction:

    ...
    !
    ! Placehoder for your Code
    !
    ! Write string value
    r_RRC_FAddString "Custom feedback";
    !
    ! Write float value
    r_RRC_FAddValue 3.14159;
    !
    ! Feedback
    ...

and on the Python side you can use the following code:

# Custom instruction 
raw_msg = abb.send_and_wait(rrc.Debug(rrc.CustomInstruction(instruction, string_values, float_values)))

# Print feedback 
print('Feedback = ', raw_msg)
1 Like