Reading gripper position over UR5's tcp/ip?

NaN

Hello,

I am able to open and close the 2F-85 gripper I have, which is mounted on an UR5, but am struggling to figure out how to read the gripper position.

Originally I had the USB-serial converter directly plugged into my computer and reading the gripper position (over /dev/ttyUSB0) was really easy (I used python, as per https://blog.robotiq.com/controlling-the-robotiq-2f-gripper-with-modbus-commands-in-python). However, then I could not coordinate the gripper with my UR5's movements.

Therefore I plugged the converter to the USB port within the UR5 control box, so that I can command the gripper in sync with the robot `movel` commands. I am able to open and close it using the library https://github.com/jkur/python-urx/blob/SW3.5/urx/robotiq_two_finger_gripper.py, or roughly:

</code><pre class="CodeBlock"><code><code>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; &nbsp; header = "def myProg():\n"<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; prog = header<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; prog += 'socket_open(\"{}\",{},\"{}\"\n)'.format("127.0.0.1",<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; 63352,<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; "gripper_socket")<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; prog += "socket_set_var(\"{}\",{},\"{}\")\n".format("ACT", 1,<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; "gripper_socket")<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; prog += "socket_set_var(\"{}\",{},\"{}\")\n".format("GTO", 1,<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; "gripper_socket")<br><br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; if str(command) == 'open':<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; msg = "socket_set_var(\"{}\",{},\"{}\")\n".format("POS", 0,<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; "gripper_socket")<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; else:<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; msg = "socket_set_var(\"{}\",{},\"{}\")\n".format("POS", 1,<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; "gripper_socket")<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; prog += msg<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; prog += <code><code><code>"end\n"<br>
self.send_program(prog)
</code>

However, the python-urx library leaves "getting the gripper status" as an exercise for the reader, and for the life of me I cannot figure out how to do it. I thought it would be so easy, since when I had it plugged into the laptop it was very straightforward. But now, I'm unclear how to read the data.

The library includes code to get the robot status (e.g. joint configuration, tool position), but as far as I can tell the robotiq gripper status is not included in that information.

Does anyone have python code for reading the gripper status over the UR5 tcp/ip protocol, or be able to give an example of how to do so?

Thank you!

NaN

For reference, the parser is at https://github.com/jkur/python-urx/blob/SW3.5/urx/ursecmon.py#L48.

And the data I can get via `self.get_all_data()` is:

<div><br></div><div>{'robot state: ': {'AdditionalInfo': {'teachButtonPressed': False<br>, 'teachButtonEnabled': True<br>, 'type': 8<br>, 'size': 9<br>}<br>, 'MasterBoardData': {'analogInputRange0': 1<br>, 'analogInputRange1': 1<br>, 'analogOutput0': 0.004000000189989805<br>, 'analogOutput1': 0.004000000189989805<br>, 'digitalOutputBits': 0<br>, 'robotVoltage48V': 48.13019943237305<br>, 'digitalInputBits': 0<br>, 'analogInput0': 0.0040280818939208984<br>, 'analogInput1': 0.0040280818939208984<br>, 'robotCurrent': 0.7877604365348816<br>, 'masterIOCurrent': 0.07211295515298843<br>, 'analogInputDomain0': 0<br>, 'analogInputDomain1': 0<br>, 'type': 3<br>, 'masterBoardTemperature': 27.6484375<br>, 'size': 75<br>}<br>, 'SecondaryClientData': {'type': 16<br>, 'size': 682<br>}<br>, 'ForceModeData': {'rx': 0.0<br>, 'ry': 0.0<br>, 'rz': 0.0<br>, 'y': 0.0<br>, 'x': 0.0<br>, 'z': 0.0<br>, 'type': 7<br>, 'robotDexterity': 0.0017944129077513415<br>, 'size': 61<br>}<br>, 'ToolData': {'toolOutputVoltage': 0<br>, 'analoginputRange2': 1<br>, 'analoginputRange3': 1<br>, 'toolVoltage48V': 461.459228515625<br>, 'toolMode': 253<br>, 'toolCurrent': 0.0<br>, 'toolTemperature': 43.0<br>, 'analogInput2': 0.06692492961883545<br>, 'analogInput3': 0.06536853313446045<br>, 'type': 2<br>, 'size': 37<br>}<br>, 'CartesianInfo': {'tcpOffsetY': 0.0<br>, 'tcpOffsetRy': 0.0<br>, 'tcpOffsetZ': 0.0<br>, 'tcpOffsetRz': 0.0<br>, 'Rx': 2.10488813323439<br>, 'Ry': -2.2127665018592837<br>, 'Rz': -0.017383455219535784<br>, 'tcpOffsetRx': 0.0<br>, 'Y': -0.0003367998686304354<br>, 'X': 0.3490712299453407<br>, 'Z': 0.24887897745368975<br>, 'type': 4<br>, 'tcpOffsetX': 0.0<br>, 'size': 101<br>}<br>, 'RobotModeData': {'isSecurityStopped': False<br>, 'speedScaling': 0.0<br>, 'isProgramPaused': False<br>, 'timestamp': 1339528000<br>, 'isRealRobotEnabled': True<br>, 'speedFractionLimit': 63<br>, 'robotMode': 7<br>, 'isProgramRunning': False<br>, 'isPowerOnRobot': True<br>, 'isPhysicalRobotConnected': True<br>, 'isEmergencyStopped': False<br>, 'speedFraction': 1.0<br>, 'type': 0<br>, 'controlMode': 0<br>, 'size': 47<br>}<br>, 'JointData': {'jointMode5': 253<br>, 'qd_actual4': 0.0<br>, 'jointMode4': 253<br>, 'T_motor0': 27.549991607666016<br>, 'T_motor5': 33.909576416015625<br>, 'size': 251<br>, 'V_actual4': 48.07600021362305<br>, 'q_target4': -1.605703<br>, 'qd_actual5': 0.015435682609677315<br>, 'V_actual0': 47.81800079345703<br>, 'V_actual1': 47.803001403808594<br>, 'V_actual2': 47.696998596191406<br>, 'V_actual3': 47.71200180053711<br>, 'q_actual3': -1.954794708882467<br>, 'q_actual2': 2.2794532775878906<br>, 'q_actual1': -1.8465498129474085<br>, 'q_actual0': -3.447080198918478<br>, 'T_motor4': 32.6035041809082<br>, 'q_actual5': -0.26228505769838506<br>, 'q_actual4': -1.605781380330221<br>, 'I_actual3': -0.2363813817501068<br>, 'I_actual2': -1.3361190557479858<br>, 'I_actual1': -1.0469255447387695<br>, 'I_actual0': -0.3183371126651764<br>, 'q_target0': -3.447025<br>, 'q_target1': -1.846558<br>, 'q_target2': 2.2794<br>, 'T_motor1': 26.4632511138916<br>, 'T_motor2': 27.639751434326172<br>, 'type': 1<br>, 'T_motor3': 31.349990844726562<br>, 'qd_actual2': -0.0<br>, 'qd_actual3': 0.0<br>, 'qd_actual0': 0.0<br>, 'qd_actual1': 0.0<br>, 'jointMode1': 253<br>, 'jointMode0': 253<br>, 'jointMode3': 253<br>, 'jointMode2': 253<br>, 'q_target3': -1.9547689999999998<br>, 'I_actual5': 0.16470444202423096<br>, 'V_actual5': 47.87900161743164<br>, 'q_target5': -0.2617989999999999<br>, 'I_actual4': -0.05490148067474365<br>, 'T_micro5': 33.551246643066406<br>, 'T_micro4': 35.86604309082031<br>, 'T_micro3': 32.943389892578125<br>, 'T_micro2': 30.52123260498047<br>, 'T_micro1': 31.656269073486328<br>, 'T_micro0': 30.62255859375<br>}<br>}<br>, '\n'<br>}</div><div></div>

However, none of this appears to correlate to the gripper position.

I'm wondering if perhaps I could issue

<div>rq_current_pos()</div><div>textmsg(rq_pos)</div>

and hope the textmsg will be picked up by get_all_data somehow? I really thought this would be straightforward...

David_Gouffe

Another related post: https://dof.robotiq.com/discussion/1382/control-grippers-from-python-using-modbus-rtu

NaN

Unfortunately, as mentioned, I am not connected directly to the gripper's usb-serial port (thus, communicating over /etc/ttyUSB0, which I did get working). Since I need to coordinate the gripper with the UR5 waypoints, which are a blended command (so several movel's in the same program, in the middle of which I open or close the gripper), I've plugged the gripper into the ur control box. However, now I cannot easily read the gripper position, or otherwise check if an object has been grasped.

Thanks though! I will post back if/when I find a solution. At this point I'm almost entertaining putting on a esp8266 or nrf24l01 with a battery and a limit switch wedged between the the V linkages... but I'll give it another shot with setting an analogout or something to the gripper pos.

NaN

I've gotten the hack of using the analogOutput on the UR to report the gripper position to work. So, in URSCRIPT I have

<div>def myProg():<br>&nbsp;&nbsp;&nbsp; socket_close("gripper_socket")<br>&nbsp;&nbsp;&nbsp; socket_open("127.0.0.1",63352,"gripper_socket")<br>&nbsp;&nbsp;&nbsp; set_analog_inputrange(0,0)<br>&nbsp;&nbsp;&nbsp; set_analog_inputrange(1,0)<br>&nbsp;&nbsp;&nbsp; set_analog_inputrange(2,0)<br>&nbsp;&nbsp;&nbsp; set_analog_inputrange(3,0)<br>&nbsp;&nbsp;&nbsp; set_analog_outputdomain(0,1)<br>&nbsp;&nbsp;&nbsp; set_analog_outputdomain(1,1)<br>&nbsp;&nbsp;&nbsp; set_tool_voltage(0)<br>&nbsp;&nbsp;&nbsp; set_runstate_outputs([])<br>&nbsp;&nbsp;&nbsp; set_payload(0.85)<br>&nbsp;&nbsp;&nbsp; socket_set_var("SPE",255,"gripper_socket")<br>&nbsp;&nbsp;&nbsp; sync()<br>&nbsp;&nbsp;&nbsp; socket_set_var("FOR",50,"gripper_socket")<br>&nbsp;&nbsp;&nbsp; sync()<br>&nbsp;&nbsp;&nbsp; sleep(0.1)<br>&nbsp;&nbsp;&nbsp; socket_set_var("POS",255,"gripper_socket")<br>&nbsp;&nbsp;&nbsp; sync()<br>&nbsp;&nbsp;&nbsp; sleep(0.1)<br><br></div><div>def analogToPos():<br>&nbsp;&nbsp;&nbsp; socket_close("gripper_socket")<br>&nbsp;&nbsp;&nbsp; socket_open("127.0.0.1", 63352, "gripper_socket")<br>&nbsp;&nbsp;&nbsp; rq_pos = socket_get_var("POS","gripper_socket")<br>&nbsp;&nbsp;&nbsp; sync()<br>&nbsp;&nbsp;&nbsp; set_standard_analog_out(0, rq_pos / 255)<br>&nbsp;&nbsp;&nbsp; textmsg(rq_pos)<br>end</div>

Of course, this is actually streamed out on my end over python. For instance, using the python-urx library linked above,

edit urx/urrobot.py<br><br>&nbsp;&nbsp;&nbsp; def set_analog_out_to_pos(self):<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; prog = "def analogToPos():\n"<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; prog += '\tsocket_close("gripper_socket")\n'<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; prog += '\tsocket_open("127.0.0.1", 63352, "gripper_socket")\n'<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; prog += '\trq_pos = socket_get_var("POS","gripper_socket")\n'<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; prog += '\tsync()\n'<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; prog += "\tset_standard_analog_out(0, rq_pos / 255)\n"<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; prog += "\ttextmsg(rq_pos)\n"<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; prog += "end"<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; self.send_program(prog)

After calling the above, I can use the python-urx library to read the analog port.

<div>&nbsp;         data = rob.secmon.get_all_data(wait=False)<br></div><div>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; print("analog state: ", data['MasterBoardData']['analogOutput0'], '\n')<br></div>

===

As a documentation of the trouble shooting process:

I downloaded the robotiq urcap (as per the "software" page) and used the ur pendant write short programs.

I saved that program to a usb, and opened it in a text editor on my computer. There I found the functions (as described in the robotiq user manual pdf). For instance there is the function --

&nbsp; def rq_current_pos(gripper_socket="1"):<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; enter_critical<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; rq_pos = socket_get_var("POS",gripper_socket)<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; exit_critical<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; sync()<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; return rq_pos<br>&nbsp; end<br>

I also used liberally, textmsg() to write values to the pendant's log. This helped debug e.g. rq_current_pos_norm outputs between [0,100] and rq_pos as defined above outputs [0,255], whereas the set_standard_analog_out desires a value between [0,1]. On the other hand, when I am reading the analog output over the so-called "ur5 matlab interface / secondary monitor / port 3002", I will get a value between [0,10].

I can also use the pendant to both view in the I/O tab, the "Analog Output', as well as drag the slider to make sure my state reader is behaving.

Finally, I realized that I need to wait a little longer (~1 sec) to read the analogoutput, since I'm not waiting until the gripper has stopped moving (for now) to proceed.

For instance,

<div>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; robotiqgrip.gripper_action(0) # open<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; time.sleep(1)<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; rob.set_analog_out_to_pos()<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; time.sleep(0.5)<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; data = rob.secmon.get_all_data(wait=False)<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; print("\n&nbsp; !----- analog state open: ", data['MasterBoardData']['analogOutput0'], '\n')<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; time.sleep(1)<br><br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; robotiqgrip.gripper_action(255)<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; time.sleep(1)<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; rob.set_analog_out_to_pos()<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; time.sleep(0.5)<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; # pose = rob.getl()<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; data = rob.secmon.get_all_data(wait=False)<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; print("\n !-----&nbsp; analog state close: ", data['MasterBoardData']['analogOutput0'], '\n')<br>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; time.sleep(1)<br></div><div><br></div><div>('\n !-----&nbsp; analog state close: ', 7.490196228027344, '\n')<br></div><div>('\n&nbsp; !----- analog state open: ', 0.11764705926179886, '\n')</div>

Now I can set my own threshold for whether an object has been picked up. Maybe in the future I will investigate the rest of the robotiq urcap functions, e.g. for object detection.

Hurray!