RTDE - get_inverse_kin()

[RobotNoob]

Hi all,
I am using an RTDE interface in python created by RopeRobotics(Python 3 library to communicate with robot), though my question is more about general RTDE communication than the interface presented above.

I know how to send commands to robot that do not require numerical output such as force_mode, servoj, movel etc.
I know how to read the defined values inside the robot (list given in RTDE guide) for example:

Problem Description
The issue of mine is that I am trying to use the defined inside the robot get_invere_kin() function to obtain the TCP pose.
What I try to do is:

1. Assign input_double_register_0-5 to store value of pose (x,y,z,Rx,Ry,Rz)
2. Send those values as an input to get_inv_kin() defined inside the robot (prog =""")
   PROBLEM
3. I don’t know how to collect the output of this function so that i can store it it in different registers which I can read on my PC. Any suggestions ?
   
   

@ajp

[ajp]

Once you have your resulting joint angle in a variable within your script program:

angles = get_inverse_kin(pose)

Write the individual values back into RTDE output registers:

write_output_float_register(0,angles[0])

should work…

[RobotNoob]

Is this what you meant? And then I use other function to readd the float registers

def get_inverse(self, pose, wait=True):
        if not self.robotConnector.RTDE.isRunning():
            self.__logger.error('RTDE need to be running to use get_inv_kin()')
            return False

    self.robotConnector.RTDE.setData('input_double_register_0', pose[0])
    self.robotConnector.RTDE.setData('input_double_register_1', pose[1])
    self.robotConnector.RTDE.setData('input_double_register_2', pose[2])
    self.robotConnector.RTDE.setData('input_double_register_3', pose[3])
    self.robotConnector.RTDE.setData('input_double_register_4', pose[4])
    self.robotConnector.RTDE.setData('input_double_register_5', pose[5])

    self.robotConnector.RTDE.sendData()

    prog = '''def ur_inverse_kin():
        while True:
            global pose = [read_input_double_register(0),
                            read_input_double_register(1),
                            read_input_double_register(2),
                            read_input_double_register(3),
                            read_input_double_register(4),
                            read_input_double_register(5)]

            angles = get_inverse_kin(p{pose})
            
            write_output_float_register(0,angles[0])
            write_output_float_register(1,angles[1])
            write_output_float_register(2,angles[2])
            write_output_float_register(3,angles[3])
            write_output_float_register(4,angles[4])
            write_output_float_register(5,angles[5])
            
            sync()
        end
    end
    '''
    programString = prog.format(**locals())

    self.robotConnector.RealTimeClient.SendProgram(programString)
    if wait:
        self.waitRobotIdleOrStopFlag()

[ajp]

Yup looks like it should work to me. Have you tried it?

[RobotNoob]

I had to write a functions that reads and writes to the registers in order to do it (because I use specific rtde package in python), I will try in next week (unfortunately i got access to the robot once a week only) and then I will post here if I succeeded or not

[ajp]

The RTDE interface is active in URSIM too, you shouldn’t need the real robot to test this.

[RobotNoob]

Hi,
Its been a while and I was experimenting more with it but without success, everytime I try to get the angles I get ‘None’ as output

[RobotNoob]

@ajp
This is what is the cause of it not working
This doesnt work, for some reason I cant call a variable that will hold a number inside the write_outpu_float_register(1,angles), it would work if i explicitly write write_output_float_register(1,2) but not with angles = 2
Any ideas why thats the case?

[ajp]

That is strange indeed. No obvious reason springs to mind just from looking at the code. Where are you seeing ‘None’?

For this type of problem, I’d recommend that you load the script into a program directly within Polyscope and execute it there. It’s a bit easier to debug that way as if there’s an issue with your code, you should see an error popup, where as you don’t get that running over the client interfaces.

[RobotNoob]

@ajp
Turns out it was an issue with indentation
Here is a working final version.
Really appreciate your help, as always your guidance was able to help me solve the problem

[ajp]

Glad to be of service