Hello there!

My objective is to control the UR5 robotic arm with the RG2 Gripper from OnRobot (link here) using Rviz for both the simulated robot in Gazebo and the Real robot itself. My current setup is Ubuntu 16.04 >> ROS Kinetic >> Gazebo V7.0.0 Using the Universal Robot and urmoderndriver packages in my catkin workspace.

As of now, i have managed to edit the .urdf files to attach the RG2 gripper model onto the UR5 in Gazebo. However, i am unsure on how to proceed further from this point in order to control the real and simulated gripper from Rviz. The only other example that i could find, is from Sharathrjtr (link here) but his RG2 gripper model is one big static mesh which i assume means that it cannot grip objects in Gazebo since his gripper hands will never close. By using his example, i am able to control the RG2 Gripper by sending,

$ rosservice call /rg2gripper/controlwidth ur_control/RG2 110

and

$ rosservice call /rg2gripper/controlwidth ur_control/RG2 0

to open and close the RG2 Gripper respectively. However, on further inspection, i find that his code just uses ROS as a means of communication with the UR5's controller (Lines 395 onwards of ur_driver.cpp)(Found here)

The parts of code are found below. Basically, all of the code assigned to cmd_str is in URScript.

std::string cmd_str;

char buf[5000],buf_socket[5000];
sprintf(buf, "\ttarget_width=%f\n",target_width);
std::cout << "Reached service to control RG2 gripper" << std::endl;

cmd_str = "def rg2ProgOpen():\n";
cmd_str += "\ttextmsg(\"inside RG2 function called\")\n";

cmd_str += buf;
cmd_str += "\ttarget_force=40\n";
cmd_str += "\tpayload=1.0\n";
cmd_str += "\tset_payload1=False\n";
cmd_str += "\tdepth_compensation=False\n";
cmd_str += "\tslave=False\n";

cmd_str += "\ttimeout = 0\n";
cmd_str += "\twhile get_digital_in(9) == False:\n";
cmd_str += "\t\ttextmsg(\"inside while\")\n";
cmd_str += "\t\tif timeout > 400:\n";
cmd_str += "\t\t\tbreak\n";
cmd_str += "\t\tend\n";
cmd_str += "\t\ttimeout = timeout+1\n";
cmd_str += "\t\tsync()\n";
cmd_str += "\tend\n";
cmd_str += "\ttextmsg(\"outside while\")\n";

cmd_str += "\tdef bit(input):\n";
cmd_str += "\t\tmsb=65536\n";
cmd_str += "\t\tlocal i=0\n";
cmd_str += "\t\tlocal output=0\n";
cmd_str += "\t\twhile i<17:\n";
cmd_str += "\t\t\tset_digital_out(8,True)\n";
cmd_str += "\t\t\tif input>=msb:\n";
cmd_str += "\t\t\t\tinput=input-msb\n";
cmd_str += "\t\t\t\tset_digital_out(9,False)\n";
cmd_str += "\t\t\telse:\n";
cmd_str += "\t\t\t\tset_digital_out(9,True)\n";
cmd_str += "\t\t\tend\n";
cmd_str += "\t\t\tif get_digital_in(8):\n";
cmd_str += "\t\t\t\tout=1\n";
cmd_str += "\t\t\tend\n";
cmd_str += "\t\t\tsync()\n";
cmd_str += "\t\t\tset_digital_out(8,False)\n";
cmd_str += "\t\t\tsync()\n";
cmd_str += "\t\t\tinput=input*2\n";
cmd_str += "\t\t\toutput=output*2\n";
cmd_str += "\t\t\ti=i+1\n";
cmd_str += "\t\tend\n";
cmd_str += "\t\treturn output\n";
cmd_str += "\tend\n";
cmd_str += "\ttextmsg(\"outside bit definition\")\n";


cmd_str += "\ttarget_width=target_width+0.0\n";
cmd_str += "\tif target_force>40:\n";
cmd_str += "\t\ttarget_force=40\n";
cmd_str += "\tend\n";

cmd_str += "\tif target_force<4:\n";
cmd_str += "\t\ttarget_force=4\n";
cmd_str += "\tend\n";
cmd_str += "\tif target_width>110:\n";
cmd_str += "\t\ttarget_width=110\n";
cmd_str += "\tend\n";
cmd_str += "\tif target_width<0:\n";
cmd_str += "\t\ttarget_width=0\n";
cmd_str += "\tend\n";
cmd_str += "\trg_data=floor(target_width)*4\n";
cmd_str += "\trg_data=rg_data+floor(target_force/2)*4*111\n";
cmd_str += "\tif slave:\n";
cmd_str += "\t\trg_data=rg_data+16384\n";
cmd_str += "\tend\n";

cmd_str += "\ttextmsg(\"about to call bit\")\n";
cmd_str += "\tbit(rg_data)\n";
cmd_str += "\ttextmsg(\"called bit\")\n";

cmd_str += "\tif depth_compensation:\n";
cmd_str += "\t\tfinger_length = 55.0/1000\n";
cmd_str += "\t\tfinger_heigth_disp = 5.0/1000\n";
cmd_str += "\t\tcenter_displacement = 7.5/1000\n";

cmd_str += "\t\tstart_pose = get_forward_kin()\n";
cmd_str += "\t\tset_analog_inputrange(2, 1)\n";
cmd_str += "\t\tzscale = (get_analog_in(2)-0.026)/2.976\n";
cmd_str += "\t\tzangle = zscale*1.57079633-0.087266462\n";
cmd_str += "\t\tzwidth = 5+110*sin(zangle)\n";

cmd_str += "\t\tstart_depth = cos(zangle)*finger_length\n";


cmd_str += "\t\tsync()\n";
cmd_str += "\t\tsync()\n";
cmd_str += "\t\ttimeout = 0\n";

cmd_str += "\t\twhile get_digital_in(9) == True:\n";
cmd_str += "\t\t\ttimeout=timeout+1\n";
cmd_str += "\t\t\tsync()\n";
cmd_str += "\t\t\tif timeout > 20:\n";
cmd_str += "\t\t\t\tbreak\n";
cmd_str += "\t\t\tend\n";
cmd_str += "\t\tend\n";
cmd_str += "\t\ttimeout = 0\n";
cmd_str += "\t\twhile get_digital_in(9) == False:\n";
cmd_str += "\t\t\tzscale = (get_analog_in(2)-0.026)/2.976\n";
cmd_str += "\t\t\tzangle = zscale*1.57079633-0.087266462\n";
cmd_str += "\t\t\tzwidth = 5+110*sin(zangle)\n";
cmd_str += "\t\t\tmeasure_depth = cos(zangle)*finger_length\n";
cmd_str += "\t\t\tcompensation_depth = (measure_depth - start_depth)\n";
cmd_str += "\t\t\ttarget_pose = pose_trans(start_pose,p[0,0,-compensation_depth,0,0,0])\n";
cmd_str += "\t\t\tif timeout > 400:\n";
cmd_str += "\t\t\t\tbreak\n";
cmd_str += "\t\t\tend\n";
cmd_str += "\t\t\ttimeout=timeout+1\n";
cmd_str += "\t\t\tservoj(get_inverse_kin(target_pose),0,0,0.008,0.033,1700)\n";
cmd_str += "\t\tend\n";
cmd_str += "\t\tnspeed = norm(get_actual_tcp_speed())\n";
cmd_str += "\t\twhile nspeed > 0.001:\n";
cmd_str += "\t\t\tservoj(get_inverse_kin(target_pose),0,0,0.008,0.033,1700)\n";
cmd_str += "\t\t\tnspeed = norm(get_actual_tcp_speed())\n";
cmd_str += "\t\tend\n";
cmd_str += "\tend\n";
cmd_str += "\tif depth_compensation==False:\n";
cmd_str += "\t\ttimeout = 0\n";
cmd_str += "\t\twhile get_digital_in(9) == True:\n";
cmd_str += "\t\t\ttimeout = timeout+1\n";
cmd_str += "\t\t\tsync()\n";
cmd_str += "\t\t\tif timeout > 20:\n";
cmd_str += "\t\t\t\tbreak\n";
cmd_str += "\t\t\tend\n";
cmd_str += "\t\tend\n";
cmd_str += "\t\ttimeout = 0\n";
cmd_str += "\t\twhile get_digital_in(9) == False:\n";
cmd_str += "\t\t\ttimeout = timeout+1\n";
cmd_str += "\t\t\tsync()\n";
cmd_str += "\t\t\tif timeout > 400:\n";
cmd_str += "\t\t\t\tbreak\n";
cmd_str += "\t\t\tend\n";
cmd_str += "\t\tend\n";
cmd_str += "\tend\n";
cmd_str += "\tif set_payload1:\n";
    cmd_str += "\t\tif slave:\n";
    cmd_str += "\t\t\tif get_analog_in(3) < 2:\n";
    cmd_str += "\t\t\t\tzslam=0\n";
    cmd_str += "\t\t\telse:\n";
    cmd_str += "\t\t\t\tzslam=payload\n";
    cmd_str += "\t\t\tend\n";
    cmd_str += "\t\telse:\n";
    cmd_str += "\t\t\tif get_digital_in(8) == False:\n";
    cmd_str += "\t\t\t\tzmasm=0\n";
    cmd_str += "\t\t\telse:\n";
    cmd_str += "\t\t\t\tzmasm=payload\n";
    cmd_str += "\t\t\tend\n";
    cmd_str += "\t\tend\n";
    cmd_str += "\t\tzsysm=0.0\n";
cmd_str += "\t\tzload=zmasm+zslam+zsysm\n";
cmd_str += "\t\tset_payload(zload)\n";
cmd_str += "\tend\n";


cmd_str += "end\n";


rt_interface_->addCommandToQueue(cmd_str);
std::cout << cmd_str << std::endl;

If possible, i would like to control the RG2 Gripper solely using ROS.

Does anyone have any experience with the RG2 Gripper? Or can at at least point me in the right direction? Please let me know if you require any additional information.~

Thanks!