Webots simultaneous movement of axis

Question

I'm using Webots to simulate the movements of a Kuka robot with 6 axes, but in my example I only have 3. My Webots controller receives the positions every 4 ms.

My problem is that using axis.setPosition() only the 1st axis in the hierarchy moves and the others may not be read. All in all, this doesn't show up on the video. That's why in a second code I used multi-threading to see if simulating all the axes at the same time would give a different result but it didn't. Can you tell me what I need to do to get all the axes moving at the same time using a timestep of 4 ms?

First Code:

from bs4 import BeautifulSoup
import socket
import math
import time
from controller import Robot


robot = Robot()
timestep = int(robot.getBasicTimeStep())

serverIP = "127.0.0.1"
serverPort = 59152
bufferSize = 1024

# Create a datagram socket
print("DataParser started")

UDPServerSocket = socket.socket(
    family=socket.AF_INET, type=socket.SOCK_DGRAM)
print("Datagram socket created")

UDPServerSocket.bind((serverIP, serverPort))
print("Address and IP binded")

# Listen for incoming datagrams
port_input = "2345"
robotName = "kr10r1420"
shadowName = "kr10r1420_shadow"

print("Connected to dataParser")
axe1 = robot.getDevice("1_axis_motor_rot")
axe2 = robot.getDevice("2_axis_motor_rot")
axe3 = robot.getDevice("3_axis_motor_rot")
axe4 = robot.getDevice("4_axis_motor_rot")
axe5 = robot.getDevice("5_axis_motor_rot")
axe6 = robot.getDevice("6_axis_motor_rot")

UDPServerSocket.settimeout(600.0)

try:
    while robot.step(4) != -1:
        
        # print("Waiting for message")
        bytesAddressPair = UDPServerSocket.recvfrom(bufferSize)
        # f.write(str(time.time()) + '\n')
        print(str(time.time()) + " Timestep: " + str(timestep) + " " + str(axe1.getAcceleration()))
        message = bytesAddressPair[0]
        address = bytesAddressPair[1]
        # print(message.decode())
        # motor.setPosition(position)
        # motor.setPosition(1)
        
        tag = BeautifulSoup(message.decode(), 'html.parser')
        # ------------------ Robot ---------------------
        aipos = tag.find_all('aipos')[0]
        a1 = math.radians(float(aipos['a1']) + 90) # axis z -> axis x
        axe1.setPosition(a1)
        # robot.step(4)   
        # a2 = math.radians(float(aipos['a2']) + 90) # axis z -> axis x
        # axe2.setPosition(a2) 
        a3 = math.radians(float(aipos['a3']) - 90) # axis x -> axis z
        axe3.setPosition(a3) 
        # robot.step(4) 
        # a4 = math.radians(float(aipos['a4'])) # It's not really defined, how it should be
        # axe4.setPosition(a4) 
        # a5 = math.radians(float(aipos['a5'])) # It's not really defined, how it should be
        # axe5.setPosition(a5) 
        a6 = math.radians(float(aipos['a6'])) # It's not really defined, how it should be
        axe6.setPosition(4)        
        
        # print("axe1 " + str(a1) + " axe2 " + str(a2) + " axe3 " + str(a3) + " axe4 " + str(a4) + " axe5 " + str(a5) + " axe6 " + str(a6))
finally:
    print("File closed")
    # f.close()

Code with threads:

from bs4 import BeautifulSoup
import socket
import math
import time
from controller import Robot
import threading


robot = Robot()
timestep = int(robot.getBasicTimeStep())

serverIP = "127.0.0.1"
serverPort = 59152
bufferSize = 1024

# Create a datagram socket
print("DataParser started")

UDPServerSocket = socket.socket(
    family=socket.AF_INET, type=socket.SOCK_DGRAM)
print("Datagram socket created")

UDPServerSocket.bind((serverIP, serverPort))
print("Address and IP binded")

# Listen for incoming datagrams
port_input = "2345"
robotName = "kr10r1420"
shadowName = "kr10r1420_shadow"

print("Connected to dataParser")
axe1 = robot.getDevice("1_axis_motor_rot")
axe2 = robot.getDevice("2_axis_motor_rot")
axe3 = robot.getDevice("3_axis_motor_rot")
axe4 = robot.getDevice("4_axis_motor_rot")
axe5 = robot.getDevice("5_axis_motor_rot")
axe6 = robot.getDevice("6_axis_motor_rot")

UDPServerSocket.settimeout(600.0)

a1 = 0
a3 = 0
a6 = 0

def update_motor1():
    while robot.step(timestep) != -1:
        axe1.setPosition(a1)

def update_motor3():
    while robot.step(timestep) != -1:
        axe3.setPosition(a3)

def update_motor6():
    while robot.step(timestep) != -1:
        axe6.setPosition(a6)

thread1 = threading.Thread(target=update_motor1)
thread3 = threading.Thread(target=update_motor3)
thread6 = threading.Thread(target=update_motor6)

thread1.start()
thread3.start()
thread6.start()


try:
    while robot.step(timestep) != -1:
        
        # print("Waiting for message")
        bytesAddressPair = UDPServerSocket.recvfrom(bufferSize)
        # f.write(str(time.time()) + '\n')
        # print(str(time.time()) + " Timestep: " + str(timestep) + " " + str(axe1.getAcceleration()))
        message = bytesAddressPair[0]
        address = bytesAddressPair[1]
        # print(message.decode())
        # motor.setPosition(position)
        # motor.setPosition(1)
        
        tag = BeautifulSoup(message.decode(), 'html.parser')
        # ------------------ Robot ---------------------
        aipos = tag.find_all('aipos')[0]
        a1 = math.radians(float(aipos['a1']) + 90) # axis z -> axis x
        # axe1.setPosition(a1)
        # robot.step(4)   
        # a2 = math.radians(float(aipos['a2']) + 90) # axis z -> axis x
        # axe2.setPosition(a2) 
        a3 = math.radians(float(aipos['a3']) - 90) # axis x -> axis z
        # axe3.setPosition(a3) 
        # robot.step(4) 
        # a4 = math.radians(float(aipos['a4'])) # It's not really defined, how it should be
        # axe4.setPosition(a4) 
        # a5 = math.radians(float(aipos['a5'])) # It's not really defined, how it should be
        # axe5.setPosition(a5) 
        a6 = math.radians(float(aipos['a6'])) # It's not really defined, how it should be
        # axe6.setPosition(4)        
        
        # print("axe1 " + str(a1) + " axe2 " + str(a2) + " axe3 " + str(a3) + " axe4 " + str(a4) + " axe5 " + str(a5) + " axe6 " + str(a6))
finally:
    print("File closed")
    thread1.join()
    thread2.join()
    thread3.join()