メモ：onRobot社のRG2GripperをURスクリプトを使って、ur_rtdeから動かしたいときのスクリプトのサンプル

ロボットアームUR5eに取り付けたonRobot社RG2グリッパーを、URScriptを使って、自前のc++プログラムから操作するときのサンプルを紹介します。

UR5eの駆動方法

UR5eを動かすための方法は、URScriptを使う(port 30000~30002)か､RTDE(Real-Time Data Exchange, port 30004)[0]を使うかに分かれます。
ROSIndustrialなどは中でURScriptを叩いているそうですが、URScriptの応答は速くなくロボットハードウェアの性能を全て引き出すことは難しいです。
ロボットの性能を引き出すために、RTDE経由でコマンドを送る方が良く、そこに着目した幾つかのグループでＲＴＤＥをラップして便利に使いやすくするライブラリの開発[1][2][3]が進められています。
一方で、onRobotのRG2はRTDEではなく、URScriptから呼び出す必要が有ります。
ここでは、開発が現在進行形で進んでいるソースコードがきれいなur_rtdeを使って、そこからURScriptを呼び出す方法を試してみたいと思います。

onRobot社のマニュアルによれば、簡単にURScriptからRG2関数を呼べるようになるのでは？

RG2関数がURScriptにロードされるのは、PolyScope内だけです。外部から呼び出すことはできません。
onRobot社のマニュアルに不備があります。

ur_rtdeのScriptClient

ur_rtdeはいくつかのコンポーネントから構成されており､RTDEReceiveInterface､RTDEControlInterface､RTDEIOInterface､ScriptClientなどがあります。
うまく使えばRTDEIOInterfaceを使ってRG2グリッパーを動かすことができるかもしれませんが、まだノウハウの蓄積がないのでわかりません。
ここでは、ScriptClientを使ってURScriptを流し込む方法で動かしてみたいと思います。

ソースコードのサンプル

ファイルに書いたスクリプトを読み込む方法と、平書きしたstringを渡す方法の2つがあります。
URScriptでは､scriptの中のタブとか改行コードとかがシビアに影響します。デバッグもし辛いので慎重に確認してください。
実機を動作させるときには、電源をＯｎにして、polyscopeのタブレットからリモートコントロールができるように設定しておく必要がありますので、忘れないでください。

ofApp.h

#pragma once

#include "ofMain.h"

#include <ur_rtde/rtde_receive_interface.h>
#include <ur_rtde/rtde_control_interface.h>
#include <ur_rtde/script_client.h>

class ofApp : public ofBaseApp{

	public:
		void setup();
		void update();
		void draw();

		void RG2(int width, int force);

		std::shared_ptr<ur_rtde::RTDEReceiveInterface> rtde_receive;
		std::shared_ptr<ur_rtde::RTDEControlInterface> rtde_control;
		std::shared_ptr<ur_rtde::ScriptClient> script_client;
};

ofApp.cpp

#include "ofApp.h"
#pragma comment(lib, "rtde.lib")

//--------------------------------------------------------------
void ofApp::setup(){
	script_client = std::make_shared<ur_rtde::ScriptClient>("192.168.1.1", 5, 5, 30002); // Polyscope ver. 5.5 port 30002

	script_client->connect();
	if (script_client->isConnected()) {
		script_client->sendScript("data/RG2.script");
		std::cout << "send command" << std::endl;
        }
}

void ofApp::update()
{
}

void ofApp::draw()
{
}

void ofApp::RG2(int width, int force)
{
	int offset_width = 9; // 9.2[mm];
	int target_width = offset_width + width;
	if (target_width > 110) target_width = 110;
	else if (target_width < 0) target_width = 0;

	if (script_client->isConnected()) {
		std::string cmd_str;
		cmd_str = "def RG2():\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\toutput = 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 += "\tdef RG2(target_width = 110, target_force = 40):\n";
		cmd_str += "\t\tbit(0)\n";
		cmd_str += "\t\tsleep(0.024)\n";
		cmd_str += "\t\trg_data = floor(target_width) * 4\n";
		cmd_str += "\t\trg_data = rg_data + floor(target_force / 2) * 4 * 111\n";
		cmd_str += "\t\trg_data = rg_data + 32768\n";
		cmd_str += "\t\tbit(rg_data)\n";
		cmd_str += "\t\twhile get_digital_in(9) == True:\n";
		cmd_str += "\t\t\tsync()\n";
		cmd_str += "\t\tend\n";
		cmd_str += "\t\twhile get_digital_in(9) == False:\n";
		cmd_str += "\t\t\tsync()\n";
		cmd_str += "\t\tend\n";
		cmd_str += "\tend\n";
		cmd_str += "\tRG2(" + std::to_string(target_width) + "," + std::to_string(rg2_force) + ")\n";
		cmd_str += "end\n";
		cmd_str += "run program\n";
		script_client->sendScriptCommand(cmd_str);

		std::cout << cmd_str << std::endl;
	}
}

RG2.script

def RG2():
	def bit(input):
		msb=65536
		local i=0
		local output=0
		while i<17:
			set_digital_out(8,True)
			if input>=msb:
				input=input-msb
				set_digital_out(9,False)
			else:
				set_digital_out(9,True)
			end
			if get_digital_in(8):
				output=1
			end
			sync()
			set_digital_out(8,False)
			sync()
			input=input*2
			output=output*2
			i=i+1
		end
		return output
	end
	def RG2(target_width=110, target_force=40):
		bit(0)
		sleep(0.024)
		rg_data=floor(target_width)*4
		rg_data=rg_data+floor(target_force/2)*4*111
		rg_data=rg_data+32768
		bit(rg_data)
		while get_digital_in(9) == True:
			sync()
		end
		while get_digital_in(9) == False:
			sync()
		end
	end
	RG2(100, 40)
end
run program