Universal Robots Forum

RTDE communication not fast enogh

I have a question about RTDE communication.
I am running a control loop on the external PC that calculates based on some math the next pose.
I then send this pose to the robot where it is read using registers, converted to joint values as feed into servoj().

I am running a check of how fast my loop runs and the first 2 iterations are very slow → 0.03 sec and 0.003 sec. (I try to communicate in 500Hz)
Only in 3rd iteration and onwards the loop is executed in 0.002sec or faster. Because of the initial delay, the servoj moves in a jerky way at the first 2 iterations.
Any ideas where this problem can come from?
Is there some delay when initializing the servoj?

idx = 1
time_step = 0.02
#   -------------------------Control loop --------------------
t_start = time.time()
while time.time() - t_start < trajectory_time:
    t_init = time.time()
    state = con.receive()   # read state from the robot
    if state.runtime_state > 1:
        t_current = time.time() - t_start
        #   ----------- minimum_jerk trajectory --------------
        if t_current <= trajectory_time:
            [position_ref, rotation, vel_ref, ang_vel, acceleration_ref] = planner.trajectory_planning(t_current)
            print(f"time is: {t_current}")
            # TODO implement rotation (currently we read x,y,z and vx,vy,vz)
            rotation_ref = [start_pose[3], start_pose[4], start_pose[5]]
            ang_vel_ref = [0, 0, 0]
        # ------------------ impedance -----------------------
        f_external = state.actual_TCP_force
        X_next = ImpedanceEq(position_ref, rotation_ref, vel_ref, ang_vel_ref, f_external, f_0, xm_pose, xmd_pose, time_step)

        xm_new = X_next[:3]
        thm_new = X_next[3:6]
        xm_d_new = X_next[6:9]
        thm_d_new = X_next[9:]

        xm_pose = [xm_new[0][0], xm_new[1][0], xm_new[2][0], thm_new[0][0], thm_new[1][0], thm_new[2][0]]
        xmd_pose =[xm_d_new[0][0], xm_d_new[1][0], xm_d_new[2][0], thm_d_new[0][0], thm_d_new[1][0], thm_d_new[2][0]]
        # ------------ sending xm (pose) values to robot -------------
        list_to_setp(setp, xm_pose)
        # print(idx)
        idx = idx + 1
        time_step = time.time() - t_init
        print(f"It takes dt = {time_step}")