Control UR robot using a joystick connected on a PC

bcastets

I would like to share with you a little program I made to control UR robot using a joystick connected on a PC.

Here is how it works:

A python program is running on my pc. It catchs the position of joystick axis and write this position in UR robot modbus server input register.
On the robot, a program is running. It calculates the next position of the robot depending on the position of the joystick and order the next move.

Python script and ur program are available on github:
https://github.com/castetsb/urJoystickControl

#Import libraries
##################
import time
import pygame
from pyModbusTCP.client import ModbusClient
import keyboard  # using module keyboard

#Initialize variables
##################
executionTime=0
MODBUS_SERVER_IP="10.20.1.108"

#Process initialization
##################

#Joystick
pygame.joystick.init()
joy=pygame.joystick.Joystick(0)
joy.init()
pygame.display.init()

#communication
# TCP auto connect on first modbus request
c = ModbusClient(host=MODBUS_SERVER_IP, port=502, auto_open=True)
c.host(MODBUS_SERVER_IP)
c.port(502)
# managing TCP sessions with call to c.open()/c.close()
c.open()

#Functions definition
def rescaleAxis(axisPosition,axisIniPosition):
  #Rescale axis position according to its initial resting position.
  #
  #Parameters:
  # axisposition: axis position of the joystick. It should be a value between -1 and 1.
  # axisIniPosition: axis position when the joystick is at rest. It should be a value between -1 and 1.
  #
  #Return:
  # rescaledPosition: axis position between -1 and 1 with a value equal to 0 when the joystick is at rest.
  rescaledPosition=0
  
  lowSegmentLen=1+axisIniPosition
  upperSegmentLen=1-axisIniPosition
  
  if axisPosition<axisIniPosition:
    rescaledPosition=((axisPosition-axisIniPosition)/lowSegmentLen)
  else:
    rescaledPosition=((axisPosition-axisIniPosition)/upperSegmentLen)
  return rescaledPosition

##################
##################
#Main program
##################
##################
#This program get the joystick position and send it to UR robot MODBUS server.
##################

#initialize Joystick position
pygame.event.pump()
axis0Ini=joy.get_axis(0)
axis1Ini=joy.get_axis(1)
axis2Ini=joy.get_axis(2)
axis3Ini=joy.get_axis(3)

while True:
  try:
    if keyboard.is_pressed('q'):  # if key 'q' is pressed 
      print('You Pressed q Key! The program stopped.')
      break
    else:
      #get joystick position
      pygame.event.pump()
      
      #right joystick
      
      #-1 for left, 1 for right
      axis0=joy.get_axis(0)
      axis0=rescaleAxis(axis0,axis0Ini)

      #-1 up 1 down
      axis1=joy.get_axis(1)
      axis1=rescaleAxis(axis1,axis1Ini)
      
      #left joystick
      
      #-1 for left, 1 for right
      axis2=joy.get_axis(2)
      axis2=rescaleAxis(axis2,axis2Ini)
      
      #-1 up 1 down
      axis3=joy.get_axis(3)
      axis3=rescaleAxis(axis3,axis3Ini)
      
      print([round(axis0,2),round(axis1,2),round(axis2,2),round(axis3,2)])
      #Send axis position to Modbus server registers 128, 129, 130 and 131
      if c.write_multiple_registers(128, [int((axis0+1)*100),int((axis1+1)*100),int((axis2+1)*100),int((axis3+1)*100)]):
        pass
      else:
        print("Error")
  except:
      break

Here is the modbus server setup on UR robot:

bcastets

Here is a video showing a UR robot controlled with this program:


https://www.youtube.com/watch?v=2wB4C9yT8NA

tdomino

hi, please can i have more explications about your program, your set up, and if your program can be uploaded in a polyscope 3.5 version ?

i try to put the program in my UR5 but there are some problems :

my set up : UR5 (192.178.1.2) + polyscope 3.5

PC(ubuntu)  + xbox360 joystick + your python program

i have a problem when i start the python program :

Traceback (most recent call last):
  File "urJoystickControl.py", line 64, in <module>
    axis3Ini=joy.get_axis(3)
pygame.error: Invalid joystick axis

when i start the UR program, the UR5 move alone ..

bcastets

This is a problem with your joystick and the integration with pygame Joystick (https://www.pygame.org/docs/ref/joystick.html).
Looking at the error message it looks like your joystick does not have axis 3.
I would recommend that you start making  a simple python program trying to get and display axis position. This way you can identify the axis of your joystick and adjust my program is needed.
The program should run under 3.5, some little adjustment maybe required.

It is a side comment but after this project, I learned that using speedL script command in the UR program would be better to have a fluid motion. With my program the robot progress step by step.

tdomino

ok thanks i will see.

can you developp your comment : " speedL script command in the UR program would be better to have a fluid motion." thanks a lot

bcastets

MoveJ and MoveL always stop when the motion is completed. Those command are use to request a position.
SpeedJ and SpeedL are used to request a speed. If you request speed the robot does not stop between each commands.
There are more details about this in UR script manual.

asagaz

Hi, please can you explain your functions : 

lowSegmentLen and upperSegmentLen : what does that mean ? 

When I run the program on the UR, there is a error message : 
"movej : unable to find a solution. The robot cannot reach this position" 

I don't understand



thanks a lot 

bcastets

@asagaz
It would be good to check what value you have for each joystick position. In my case the joystick position was in the range [-1;1] if you have a larger range like [-100,100]. If you have this kind of range, you will have to adjust the calculation formula to make sure you request possible displacement to the robot.
In your case, it looks like you request a long displacement (like +2m) and the robot says that he cannot make it.

asagaz

@bcastets
i have a range [-1;1] for all axis except the axis 3 which has a range [0:1] 

Can you explain why you calculate r,p and yaw like this : d2r(5*(x-1)/100) ?
and after when you calculate the end pose : -0.05*(R_upDown-100)/100 ?

Thanks a lot 

bcastets

@asagaz
lowSegmentLen and upperSegmentLen : what does that mean ? 

This is range of variation for negative and positive value of the joystick.

lowSegmentLen=1+axisIniPosition
upperSegmentLen=1-axisIniPosition

Example:
The neutral position of the joystick is 0.2.
The length of the lower range is 0.2-(-1)=1.2
The length of the upper range is 1-0.2=0.8

I then use those upper and lower range to calculate the position of the joystick in the upper and lower range.

Example:
The position of the Joystick is 0.4.
It is (0.4-0.2)/0.8=25% of upper range.

I send the date to Robot modbus register in the range [0;200]. So I convert the range [-1;1] to [0;200]

Then in the UR program I access the value of modbus register and convert it back in the range [-1;1].

(x-100)/100<br>

In the formula d2r(5*(x-100)/100), the 5* is just a factor to increase the angle range to [-5,5] degree. You can change this factor as you like to get the reaction you want.

-0.05*(R_upDown-100)/100 is similar. -0.05* is a factor use to lower the reaction and reverse it. You can adjust it as you like.

asagaz

Ok i see thank a lot for your help

tyty

Hi, thank you for this post. It is what I want to do right now. But seems that I cannot built the connection. Error shows that MODBUS Signal is not connected. Somay I ask the UR is only connected with pc via a ethernet cable?

bcastets

@tyty
It should both possible to connect to the robot via local network DHCP router or directly with an ethernet cable.
If you connect directly, you have to set a fixed IP for the robot and your PC. This is same as connecting 2 PC together via ethernet.
If you can I would recommend to contact your IT department to get some help.

tyty

Hi

Thank you for your reply. That's exactly what I have done. I set the static ip for my pc and got the successful connection because I can control UR via Robotdk. So it makes me confused that I cannot get the modbus through. The status indicators are all gray. Please let me know if you have further suggesstions.

tyty

Hi I have sorted the connection problem. Thank you for your help.

tyty

Hi, I have got another issue, so thank you in advance for your time and patience. When I run the program in my pc, the ur will move by itself, and I can see the position printed by 'print([round(axis0,2),round(axis1,2),round(axis2,2),round(axis3,2)])', is not all zeros after I release the joystick at rest. My joy stick is xbox one. Ask for your suggestion for which part may cause this kind of issue?

bcastets

@tyty
This is normal. The neutral position of your joystick is not a perfectly at zero. You have to measure the position returned at neutral position and offset position value to get a perfect zero. I have this offset in my code.

asagaz

Hello @bcastets , 

Now i want to use speedl in order to have a fluid motion. But when I change moveJ by speedl, the UR move alone.
Do you know what we need to change on your code to use speedl ? 

Thanks a lot

bcastets

@asagaz
movel is moving the robot from one point to another while speedl is move the robot at a the requested speed in the requested direction without distance limit. Knowing this you should be able to change the code.

asagaz

Thank you my code is finish.

I want to take screenshot on the polyscope but i don't know how to do. 

Can you help me ? 

bcastets

@asagaz
Congratulation !
You can remote control the robot from your PC using VNC. Then you can take screenshot from your PC.
https://forum.universal-robots.com/t/remote-desktop-to-the-controller-ui/3826

asagaz

Ok or how do you do to transfer the script file of your code on your PC ? Because my PC cannot read urp file.

thanks a lot

bcastets

You can access robot file system via FTP client like https://filezilla-project.org/
A program comes in 3 files:

• urp
• txt
• script

Only urp file is used to run the program but you can check the script file to get the translation of the program in script.

The txt file is just a txt version of the program which you can use to visually review the program.