Swift Environment Elements Do Not Update

[fvatansever]

Hello,

I have written a small program -by being inspired by one of your examples- with the toolbox using Swift for manual jogging with UR5. The GUI can be seen below;

Besides the joint angles, I have added more sliders to the program including, X, Y, Z, Roll, Pitch and Yaw. Also these sliders are given below.

Each slider works perfectly fine on their own. The problem I am having right now is that, when I make the robot move from the X,Y,Z, Roll, Pitch and Yaw sliders, the joint sliders do not update. I tried using the env.elements.update() function but it does not seem to work. Could you give any advice for this problem? The code I have used is given below,

from unittest import expectedFailure
import swift
import spatialmath as sm
import roboticstoolbox as rtb
import numpy as np
import time

# Launch the simulator Swift
env = swift.Swift()
env.launch()

# Make a ur5 robot and add it to Swift
ur5 = rtb.models.UR5()
ur5.q = ur5.qz
upt=False
env.add(ur5)

# This is our callback funciton from the sliders in Swift which set
# the joint angles of our robot to the value of the sliders
def set_joint(j, value):
    ur5.q[j] = np.deg2rad(float(value))

def set_pos_x(value):   
    p_try=ur5.fkine(ur5.q)
    p_try.t[0]=value
    sol=ur5.ikine_LM(p_try,q0=ur5.q)
    if sol.success: ur5.q=sol.q

def set_pos_y(value):
    p_try=ur5.fkine(ur5.q)
    p_try.t[1]=value
    sol=ur5.ikine_LM(p_try,q0=ur5.q)
    if sol.success: ur5.q=sol.q

def set_pos_z(value):
    p_try=ur5.fkine(ur5.q)
    p_try.t[2]=value
    sol=ur5.ikine_LM(p_try,q0=ur5.q)
    if sol.success: ur5.q=sol.q

def set_rot_r(value):
    p_act=ur5.fkine(ur5.q)
    p_try= sm.SE3(p_act.t[0],p_act.t[1],p_act.t[2]) * sm.SE3.RPY(np.deg2rad(value),p_act.rpy()[1],p_act.rpy()[2])
    sol=ur5.ikine_LM(p_try,q0=ur5.q)
    if sol.success: ur5.q=sol.q

def set_rot_p(value):
    p_act=ur5.fkine(ur5.q)
    p_try= sm.SE3(p_act.t[0],p_act.t[1],p_act.t[2]) * sm.SE3.RPY(p_act.rpy()[0],np.deg2rad(value),p_act.rpy()[2])
    sol=ur5.ikine_LM(p_try,q0=ur5.q)
    if sol.success: ur5.q=sol.q

def set_rot_y(value):
    p_act=ur5.fkine(ur5.q)
    p_try= sm.SE3(p_act.t[0],p_act.t[1],p_act.t[2]) * sm.SE3.RPY(p_act.rpy()[0],p_act.rpy()[1],np.deg2rad(value))
    sol=ur5.ikine_LM(p_try,q0=ur5.q)
    if sol.success: ur5.q=sol.q

def bt(value):
    global upt
    if not upt:
        upt=not upt

env.add(swift.Button(bt,desc="toogle control"))

# Loop through each link in the ur5 and if it is a variable joint,
# add a slider to Swift to control it
j = 0
for link in ur5.links:
    if link.isjoint:

        # We use a lambda as the callback function from Swift
        # j=j is used to set the value of j rather than the variable j
        # We use the HTML unicode format for the degree sign in the unit arg
        env.add(
            swift.Slider(
                lambda x, j=j: set_joint(j, x),
                min=np.round(np.rad2deg(link.qlim[0]), 2),
                max=np.round(np.rad2deg(link.qlim[1]), 2),
                step=1,
                value=np.round(np.rad2deg(ur5.q[j]), 2),
                desc="UR5 Joint " + str(j),
                unit="°",
            )
        )

        j += 1

px=env.add(
    swift.Slider(
        set_pos_x,
        min=-2,
        max=2,
        step=0.001,
        value=ur5.fkine(ur5.q).t[0],
        desc="UR5 X",
        unit=" m",
        )
    )        
py=env.add(
    swift.Slider(
        set_pos_y,
        min=-2,
        max=2,
        step=0.001,
        value=ur5.fkine(ur5.q).t[1],
        desc="UR5 Y",
        unit=" m",
        )        
    )
pz=env.add(
    swift.Slider(
        set_pos_z,
        min=-2,
        max=2,
        step=0.001,
        value=ur5.fkine(ur5.q).t[2],
        desc="UR5 Z",
        unit=" m"
        )        
    )

rr=env.add(
    swift.Slider(
        set_rot_r,
        min=-180,
        max=+180,
        step=0.01,
        value=np.round(np.rad2deg(ur5.fkine(ur5.q).rpy()[0]),2),
        desc="UR5 Roll",
        unit="°"
        )        
    )

rp=env.add(
    swift.Slider(
        set_rot_p,
        min=-180,
        max=+180,
        step=0.01,
        value=np.round(np.rad2deg(ur5.fkine(ur5.q).rpy()[1]),2),
        desc="UR5 Pitch",
        unit="°"
        )        
    )

ry=env.add(
    swift.Slider(
        set_rot_y,
        min=-180,
        max=+180,
        step=0.01,
        value=np.round(np.rad2deg(ur5.fkine(ur5.q).rpy()[2]),2),
        desc="UR5 Yaw",
        unit="°"
        )        
    )

while True:
    # Process the event queue from Swift, this invokes the callback functions
    # from the sliders if the slider value was changed
    # env.process_events()

    # Update the environment with the new robot pose
    env.step(0)
    if upt:
        upt=False
        env.elements.update()

    time.sleep(0.01)

Thanks in advance.