Using ROS Rviz to control the UR5 with OnRobot RG2 Gripper

edit

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 ur_modern_driver 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 /rg2_gripper/control_width ur_control/RG2 110

and

$ rosservice call /rg2_gripper/control_width 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 ...