Updating Output registers LIVE

[ali.mohammadijou_1]

Dear Forum,

I am working with a UR5. I have defined a custom feature plane called “display”.
I would like to convert the pose wrt Base to the Posewrt Feature and write the X, Y and Z of the result to Output_Float_Registers [0], [1] and [2]. and then read these values back in real time via RTDE.
Solution I came up with: Add a thread within the programs which does this. Output_Float_Registers get updated. Please see photo.
But what if I run a MoveL command via scripting (NOT a program) and want the values get updated the same way ?
Is there a way to globally define values of Output_Registers and let them be updated automatically without the need for assigning them each time ?

[eric.feldmann]

You can name the Register in the IO section of the Installation. This can at least allow you to use a basic Set command to do the assignment. But you still have to assign values to the output registers if you want them to change. Otherwise I reckon you’d have to use something like a Python Daemon that utilizes the RTDE to just read the actual TCP value and do the (probably) complicated math of transforming the pose outside of URScript.

What exactly is wrong with how you’re currently doing it?

[ali.mohammadijou_1]

Hi Eric,

Thanks for the comment. In the I/O section I can only set Digital or Analog OUTs which does not help much to store actual Pose values.

So the best would be to store them in the Output_Float_Registers since RTDE has access to them at 500 Hz. The thing is that I want to always know the TCP position wrt my feature plane. But get_actual_tcp_pose() will always give you the position wrt Base frame. So some conversion must always take place.

But imagine a scenario in which you use a simple MoveL command via scripting, and throughout your movement, you want to know you current position wrt to feature LIVE via RTDE.
RTDE has access to get_actual_tcp_pose () in Base frame naturally. But I want to use Rtde Output Float Registers to store conversion values wrt feature. But since I am only running the script MoveL, I am not assigning those values LIVE.

[eric.feldmann]

OK so to start, yes you CAN Set the general purpose registers with the Set command, you just have to NAME them something in the IO Setup screen first. Then they show up in the dropdown list of Inputs/Outputs.

EDIT: Reading a little closer, you say not RUNNING a program. I misread that initially as not using Polyscope. So you’re saying you’re controlling the robot by some other method. There’s a lot of other ways to control the robot, so knowing how you’re doing that will help guide an answer

[ali.mohammadijou_1]

Eric,

Indeed i also misunderstood your approach, initially i thought you means these I/Os:

But obviously you meant the GP Output Registers. those are exactly the ones which I initially meant So I wanna store the Converted pose values in the Float Output registers LIVE, without needing to assign them continuously. And access them LIVE via RTDE. RTDE has access to those output registers.

Some background info: I am using LabVIEW to control the robot. Robot is a small part of our test sequence (which is fully written in LabVIEW). So it should be fully integrated in our LabVIEW framework. For controlling the robot I use the SDK driver provided by UnderAutomation. and for controlling, I use the PrimaryInterface>Script>Send module. which simply sends Scripts to the robot, for instance a MoveL command. on a separate LabVIEW VI, I turn on RTDE at 500 Hz and read the registers LIVE. These reading will be my input to control the robot via PrimaryInterface scripting.

[eric.feldmann]

Gotcha. Well you’re outside my area of expertise then I’ve never worked with LabVIEW so can’t offer any solution to that. Is it possible to add the thread and start it running alongside every other script you send it? So like I understand you just want to send a moveL, but could you send the whole script code for that thread at the start, set it running, then send your MoveL? I’m not sure if you can send and run threads over the primary interface. I’ve never tried.

Otherwise I think you’re probably back to what I alluded to at the beginning. If you’re getting the LIVE values in LabView, you might have to create your own routine OUTSIDE the robot to take those values and transform them into the Frame of your feature. You might be able to find some code to do that (whether that be Python, or Labview or something else)

[mmi]

If you can do the transformation in labview then RTDE is all the time streaming actual TCP pose wrt base - no script needed.
Otherwise this approach is correct. Note that thread needs to run just once per cycle - add sync() in the thread loop.

[ali.mohammadijou_1]

Hi mmi,

Thanks for your input. So I tried to activate a thread to update the float register as shown in my first post, and execute a MoveL command. So I send this script via Primary Interface.

“thread Thread_1():
while (True):
$ 9 “posewrtfeature?pose_trans(pose_inv(display_const),get_actual_tcp_pose())”
global posewrtfeature= pose_trans ( pose_inv (p[0.5391674406968558,0.2501721015613766,0.3385201982265926,0.7230735002351665,-0.7705353908790852,-1.3823487705853308]), get_actual_tcp_pose ())
$ 10 “write_output_float_register(0,posewrtfeature[0])”
write_output_float_register(0,posewrtfeature[0])
$ 11 “write_output_float_register(1,posewrtfeature[1])”
write_output_float_register(1,posewrtfeature[1])
$ 12 “write_output_float_register(2,posewrtfeature[2])”
write_output_float_register(2,posewrtfeature[2])
end
end
threadId_Thread_1 = run Thread_1()
movel(pose_add(get_target_waypoint(),pose_sub(pose_trans(p[0.5391674406968558,0.2501721015613766,0.3385201982265926,0.7230735002351665,-0.7705353908790852,-1.3823487705853308],p[0.020000000,0.000000000,0.000000000,0,0,0]),p[0.5391674406968558,0.2501721015613766,0.3385201982265926,0.7230735002351665,-0.7705353908790852,-1.3823487705853308])), a=1.0, v=0.03)”

So what I see, The MoveL command get executed and robot moves, but I dont see the Output_Float_Register[0,1,2] getting updated on RTDE side. But When I run a program like in my first post, they do get updated. Do I miss something ?

[mmi]

This thread needs to be a part of the main program.
Our software can not execute standalone threads.
Are you also sending “MoveL” program to primary interface?

In this case “move” needs to be wrapped in program together with thread:

def moving_and_streaming():
  # define reference feature
  display_const=p[...]
  # start thread streaming transformed poses
  thread stream_pose():
     ...
   end
   run stream_pose()
   # execute move
   movel(...)
end

[ali.mohammadijou_1]

So I tried to wrap everything in a program as you suggested, Still i encounter the same behavior, the “MoveL” command works and robot moves, but Output_Float registers don’t get updated. And yes I use the primary interface to send commands.

This is the command based on your advice:

def moving_and_streaming():
thread stream_pose():
while (True):
global posewrtfeature= pose_trans ( pose_inv (p[0.5391674406968558,0.2501721015613766,0.3385201982265926,0.7230735002351665,-0.7705353908790852,-1.3823487705853308]), get_actual_tcp_pose ())
write_output_float_register(0,posewrtfeature[0])
write_output_float_register(1,posewrtfeature[1])
write_output_float_register(2,posewrtfeature[2])
end
end
run stream_pose()
movel(pose_add(get_target_waypoint(),pose_sub(pose_trans(p[0.5391674406968558,0.2501721015613766,0.3385201982265926,0.7230735002351665,-0.7705353908790852,-1.3823487705853308],p[0.020000000,0.000000000,0.000000000,0,0,0]),p[0.5391674406968558,0.2501721015613766,0.3385201982265926,0.7230735002351665,-0.7705353908790852,-1.3823487705853308])), a=1.0, v=0.03)
end

[eric.feldmann]

This is exactly the script I was imagining you sending the robot. I also feel like I’ve tried something similar and saw the same results you’re seeing now. I just chalked it up as the primary interface being unable to run threads AT ALL and gave up/moved on. I’d be curious to know if there’s something we’re both missing here.

[mmi]

I just tried following - slightly simpler script, and it works perfectly.

def moving_and_streaming():
    thread send_position():
      while(True):
        local position = get_actual_tcp_pose()
        write_output_float_register(0, position[0])
        write_output_float_register(1, position[1])
        write_output_float_register(2, position[2])
        sync()
      end
    end

    # first move to home position
    movej([0, -1.4, -2, -1.57, 1.6, 0], a=1, v=1)

    # start a thread streaming the position, and execute actual motion
    run send_position()
    movel(p[0.5, 0.5, 0.1, 3.15, 0, 0], a=0.1, v=1)
end

It’s sent to primary interface. First it moves robot to some known position, and then starts thread and streams xyz tcp pose (not transformed, but hopefully proves the point)

Note that indentations are important in programs sent to primary interface. This is actually mentioned in script manual. First def moving_and_streaming(): and last end must not have any leading white space characters.
All other lines in between must have at least one leading white space character. Otherwise primary interface compiles each line individually.

[ali.mohammadijou_1]

Deaar mmi,

thanks for your reply,
I tried it and it worked for me as well, indeed the trick was in the leading whitespaces.

Regards,
Ali

[ali.mohammadijou_1]

Hi mmi,

I send the following script to create a MoveL action and at the same time stream converted positions to Double Output Regsiters.

“def moving_and_streaming():
thread send_position():
while(True):
local position = pose_trans(pose_inv(p[0.4885084332082746,0.22652484714191604,0.3729249328877672,0.7542408940800382,-0.7560928630197719,-1.439516258072408]),get_actual_tcp_pose())
write_output_float_register(0, position[0])
write_output_float_register(1, position[1])
write_output_float_register(2, position[2])
write_output_float_register(3, position[3])
write_output_float_register(4, position[4])
write_output_float_register(5, position[5])
sync()
end
end
run send_position()
movel(pose_add(get_target_waypoint(),pose_sub(pose_trans(p[0.4885084332082746,0.22652484714191604,0.3729249328877672,0.7542408940800382,-0.7560928630197719,-1.439516258072408],p[0.010000000,0.000000000,0.000000000,0,0,0]),p[0.4885084332082746,0.22652484714191604,0.3729249328877672,0.7542408940800382,-0.7560928630197719,-1.439516258072408])), a=1, v=0.030)
end”

Both parts of this command (Streaming and Moving) work fine. the issue is that once the Robot stops moving, I have to wait minimum 200 ms before sending the next MoveL command. Otherwise the robot will go into a maniac state until it hits a wall.

Can you spot any issues in here ? Do I cause some race conditions or something ? Because I need the robot to perform consecutive MoveL commands almost without delay.

Thanks a lot

[mmi]

I guess that get_target_waypoint() is used incorrectly. This is mostly useful when robot is during the motion, not before starting.
Is it your intention to use current pose instead (get_target_tcp_pose())?