controlling irc5 with Python using EGM (robotstate 1.1)

ofirdva

Hello everyone, 
as written in the topic, I'm trying to send EGM_START_POSE a value to move my end effector to a spacific position and orientation. 
I downloaded the state machine add in which providing TRobEGM module:

       Provides support of using the following Externally Guided Motion (EGM) features:

       - EGM UDP unicast variant (i.e. type of communication), with the following modes:

         - EGM joint mode (for controlling motion).

         - EGM pose mode (for controlling motion).

         - EGM stream mode (for streaming position data).

 I run this program i wrote to initialize the procedure:

MODULE MainModule

    PROC main()

        initializeEGMModule;  ! Initialize the EGM module

        WHILE TRUE DO

            runEGMRoutine;  ! Run the EGM routine to handle external commands

        ENDWHILE

    ENDPROC

ENDMODULE

I have connected the robot to my pc by ethernet cable and after sending command via RWS (like turnning the motors on/off and running basic rapid program) i decided to control it from Python.
this is the script:

import socket

import struct

import time

import math

def main():

    robot_ip = "192.168.125.1"  # Robot controller IP

    port = 6511  # Port for ROB_1 (EGM Pose Mode)

    

    udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

    sequence_id = 1  # Initialize Sequence ID

    # Example Cartesian pose: X (mm), Y (mm), Z (mm), Rx (rad), Ry (rad), Rz (rad)

    position = [300.0, 200.0, 400.0]  # Position (X, Y, Z) in mm

    orientation = [0.0, math.radians(30), math.radians(45)]  # Orientation in radians

    try:

        while True:

            timestamp = time.time()

            # Pack header: Sequence ID (uint32) and timestamp (double)

            header = struct.pack('>I d', sequence_id, timestamp)

            # Pack pose data: X, Y, Z, Rx, Ry, Rz (6 doubles)

            pose_data = struct.pack('>6d', *(position + orientation))

            # Combine header and pose data

            message = header + pose_data

            # Send the UDP packet

            udp_socket.sendto(message, (robot_ip, port))

            print(f"Sent pose: {position + orientation} | Seq ID: {sequence_id}")

            sequence_id = (sequence_id + 1) % (2**32)  # Wrap sequence ID

            time.sleep(0.004)  # Send at 250Hz (4ms)

    except KeyboardInterrupt:

        print("Streaming stopped.")

    finally:

        udp_socket.close()

if __name__ == "__main__":

    main()

After Establishing RWS connection and turning the motors on, I run the rapid program which waits for position and orientation so I run the Python scrupt but the robot stands still. I alreday configured the IP Adresses and Remote Port Numbers. 
Can someone tell what am I missing? 
Thank for the help!

johnaltitude

I'm not too familiar with the EGM system, but my first thought is that in most cases ABB robots use quaternions for orientation, not XYZ euler angles. It's worth double checking, at least, if that's the orientation format it's expecting.