choreonoid用openrtm連携プラグイン python版チュートリアル(tankjoystick)

このページではChoreonoid OpenRTM連携プラグイン Python版でTankモデルをゲームパッドで操作するまでの手順を説明します。使用するモデル、作成するRTCはChoreonoid公式ページのチュートリアルとほぼ同じです。

#contents

RTC作成

この章ではシミュレータ上のアクチュエータ、センサなどの入出力を行うRTCの開発手順を説明します。

RTC Builder起動

まずはRTC Builderでソースコードのひな型を作成します。 RTC Builder利用のために、OpenRTM-aistをインストールしてください。

OpenRTM-aist.2.0.1-RELEASE

インストールが完了したらRTC Builderを起動してひな型を作成してください。 Windows 8.1の場合は「スタート」>「アプリビュー(右下矢印)」>「OpenRTM-aist 2.0.1」>「OpenRTP」をクリックすると起動できます。

作成手順は以下が参考になります。

RTCBuilder-1.1.0

RTCひな型作成

作成するRTCの仕様は以下のようになっています。言語はPythonを選択してください。

コンポーネント名称	TankIoRTC_Py
CENTER: InPort
ポート名	velocities
型	TimedVelocity2D
説明	車体の目標速度
InPort
ポート名	torques
型	TimedDoubleSeq
説明	砲塔部分の関節トルク
InPort
ポート名	lightSwitch
型	TimedBooleanSeq
説明	ライトのオンオフ
OutPort
ポート名	angles
型	PanTiltAngles
説明	砲塔部分の関節角度
言語
Python

ソースコード編集

生成したソースコードにRTCのモジュール名のクラス(TankIoRTC_Pyならばclass TankIoRTC_Py)が記述されているので、そのクラスに以下の関数を追加してください。

関数名	引数	内容
setBody	body	Bodyオブジェクトを設定する関数
inputFromSimulator		センサの計測値などをアウトポートから出力する処理等を記述する関数。シミュレーションステップ後に実行される
outputToSimulator		アクチュエータのトルクなどをインポートから入力する処理等を記述する関数。シミュレーションステップ前に実行される

具体的には以下のソースコードを記載します。

 class TankIoRTC_Py(OpenRTM_aist.DataFlowComponentBase):
 	(省略)
 
 	#ボディオブジェクト設定関数
 	def setBody(self, body):
 		
 		self.ioBody = body
 		
 		#Linkオブジェクト取得
 		self.cannonY = self.ioBody.link("CANNON_Y")
 		self.cannonP = self.ioBody.link("CANNON_P")
 		self.crawlerL = self.ioBody.link("CRAWLER_TRACK_L")
 		self.crawlerR = self.ioBody.link("CRAWLER_TRACK_R")
 		
 		#Lightオブジェクト取得
 		self.light = self.ioBody.getLight("MainLight")
 		
 		
 	#センサの計測値などをアウトポートから出力する処理等を記述する関数
 	#シミュレーションステップ後に実行される
 	def inputFromSimulator(self):
 		if self.ioBody:
 			#砲台の角度取得、格納
 			self._d_angles.pan = self.cannonY.q
 			self._d_angles.tilt = self.cannonP.q
 
 			#砲台の角度出力
 			OpenRTM_aist.setTimestamp(self._d_angles)
 			self._anglesOut.write()
 
 
 	#アクチュエータのトルクなどをインポートから入力する処理等を記述する関数
 	#シミュレーションステップ前に実行される
 	def outputToSimulator(self):
 		if self.ioBody:
 			#砲台のトルク入力
 			if self._torquesIn.isNew():
 				data = self._torquesIn.read()
 				self.cannonY.u = data.data[0]
 				self.cannonP.u = data.data[1]
 			#車体の速度入力
 			if self._velocitiesIn.isNew():
 				data = self._velocitiesIn.read()
 				vx = data.data.vx
 				va = data.data.va
 				
 				rms = (vx + va*self._wheel_distance[0])/self._wheel_radius[0]
 				lms = (vx - va*self._wheel_distance[0])/self._wheel_radius[0]
 				
 				#クローラーの速度入力
 				self.crawlerL.dq = lms
 				self.crawlerR.dq = rms
 			#ライトのオンオフ入力
 			if self._lightSwitchIn.isNew():
 				data = self._lightSwitchIn.read()
 				
 				self.light.on(data.data[0])
 				self.light.notifyStateChange()
 
 

まず、setBody関数内で制御対象のLinkオブジェクト、Lightオブジェクトを取得しています。 Link、Lightオブジェクトは名前で取得できます。

Link名はモデルに対応するアイテムを選択後、左下のビューから”“リンク”“タブを表示すれば確認できます。

Light名の確認方法は分からないのでChoreonoid公式サイトを確認してください。

 	def setBody(self, body):
 		self.ioBody = body
 
 		self.cannonY = self.ioBody.link("CANNON_Y")
 		
 		(省略)
 		
 		self.light = self.ioBody.getLight("MainLight")

関節の角度の取得q変数により取得できます。

 self._d_angles.pan = self.cannonY.q

関節の速度の入力はdq、トルクの入力はu変数に格納することでシミュレータに反映できます。

 self.crawlerL.dq = lms

 self.cannonY.u = data.data[0]

ライトのオンオフはon変数にbool変数を格納します。

 self.light.on =  data.data[0]

RTシステム作成

Choreonoid起動

Windows

Windowsの場合はChoreonoidを展開したフォルダのbin/chorenoid.exeをダブルクリックしてください。

Ubuntu

Ubuntuの場合はコマンドからchoreonoidと入力してください。

アイテム追加

ワールド、シミュレータ

まずはワールドアイテム、シミュレータアイテムを追加します。ファイル、新規からワールドとAISTシミュレータを選択して追加してください。 ※表示されるアイテムの順番が変わることがあるため、この画像と違う画面になる可能性があります。

この時点でアイテムツリーは以下のようになります。

 World(ワールドアイテム)
    |-AISTSimulator(AISTシミュレータ)

アイテムツリーの概要、アイテムの移動方法は以下のページを参考にしてください。

プロジェクトとアイテム — Choreonoid 開発版ドキュメント

モデル

次に環境、タンクのモデルを追加します。

ファイル、読み込みからOpenHRP モデルファイルを選択後、以下のファイルを読み込んでください。

{Choreonoidインストールディレクトリ}/share/model/tank/tank.wrl
{Choreonoidインストールディレクトリ}/share/model/Labo1/Labo1.wrl

この時点でアイテムツリーは以下のようになります。

 World(ワールドアイテム)
    |-AISTSimulator(AISTシミュレータ)
    |-Tank(model/tank/tank.wrl)
    |-Labo1(model/Labo1/Labo1.wrl)

RTコンポーネント

RTコンポーネントを追加します。ファイル、新規からPyRTCを選択して追加してください。

Tankアイテムの下に3つのアイテムを追加して、TankIO、Controller、Joystickと名前を付けてください。

この時点でアイテムツリーは以下のようになります。

 World(ワールドアイテム)
    |-AISTSimulator(AISTシミュレータ)
    |-Tank(model/tank/tank.wrl)
      |-TankIO(PyRTC)
      |-Controller(PyRTC)
      |-Joystick(PyRTC)
    |-Labo1(model/Labo1/Labo1.wrl)

Pythonファイルの設定

TankIO、Controller、JoystickのプロパティからRTC moduleという項目を設定してください。

各アイテムで以下のファイルを設定します。

アイテム名	ファイル名
TankIO	TankIoRTC_Py.py
Controller	TankJoystickControllerRTC_Py.py
Joystick	JoystickPySDL2.py

Pythonファイルを設定するとRTCが起動します。 ※本プラグインではChoreonoidに付属しているC++版OpenRTMプラグインのようにControllerRTC、BodyIoRTC、RTCという区別はありません。RTCのソースコードにシミュレータ上のロボットの制御、センサ値の取得等の処理を書けばそれでBodyIoRTCと同じ動作ができます。

RTシステム構築

RTシステムを追加します。ファイル、新規からRTシステムを選択して追加してください。

表示、ビューの表示から、RTCダイアグラムを表示してください。

‘RTCリストが表示されていない場合は同様に表示してください。

RTCダイアグラム表示後、左下のRTCリストからドラッグアンドドロップすることで、RTCダイアグラム上にRTCを表示できます。

RTCダイアグラム上で以下のように接続してください。

シミュレーション実行

シミュレーションを開始する前に、時間分解能を1000fpsに設定してください。これでシミュレーションを開始するとゲームパッドのジョイスティックでシミュレータ上のクローラー、アームの操作、ボタンでライトのオンオフを操作できるようになります。

RTCの仕様

基本はChoreonoid付属のOpenRTMプラグインのサンプルと同じですが、一部データ型の変更、コンフィギュレーションパラメータの追加を行っています。

TankJoystickControllerRTC_Py

Tankロボットの制御を行うためのRTCです。

コンポーネント名称	TankJoystickControllerRTC_Py
InPort
ポート名	angles
型	PanTiltAngles
説明	砲塔部分の関節角度
InPort
ポート名	axes_1
型	TimedVector2D
説明	右アナログスティックの状態。傾けると0.0～1.0の範囲で値を出力する。傾けた方向で右が正、左が負、下が正、上が負の値になります。
InPort
ポート名	axes_2
型	TimedVector2D
説明	左アナログスティックの状態
InPort
ポート名	buttons
型	TimedBooleanSeq
説明	ボタンのオンオフ
OutPort
ポート名	velocities
型	TimedVelocity2D
説明	車体の目標速度
OutPort
ポート名	torques
型	TimedDoubleSeq
説明	砲塔部分の関節トルク
OutPort
ポート名	lightSwitch
型	TimedBooleanSeq
説明	ライトのオンオフ
Configuration
パラメーター名	timeStep
型	double
デフォルト値	0.001
説明	シミュレーションのステップ時間
CENTER: Configuration
パラメーター名	KP
型	double
デフォルト値	200.0
説明	比例ゲイン
Configuration
パラメーター名	KD
型	double
デフォルト値	50.0
説明	微分ゲイン

JoystickPySDL2

ゲームパッドのアナログスティック、ボタン等の状態を出力するRTCです。

コンポーネント名称	TankJoystickControllerRTC_Py
OutPort
ポート名	axes_1
型	TimedVector2D
説明	右アナログスティックの状態。傾けると0.0～1.0の範囲で値を出力する。傾けた方向で右が正、左が負、下が正、上が負の値になります。
OutPort
ポート名	axes_2
型	TimedVector2D
説明	左アナログスティックの状態
OutPort
ポート名	buttons
型	TimedBooleanSeq
説明	ボタンのオンオフ
OutPort
ポート名	hats
型	TimedBooleanSeq
説明	十字キーの状態
OutPort
ポート名	balls
型	TimedVector2D
説明	ジョイボールの移動量
Configuration
パラメーター名	index
型	int
デフォルト値	0
説明	ゲームパッドのID

choreonoid用openrtm連携プラグイン python版 チュートリアル(tankjoystick)

RTC作成

RTC Builder起動

RTCひな型作成

ソースコード編集

RTシステム作成

Choreonoid起動

Windows

Ubuntu

アイテム追加

ワールド、シミュレータ

モデル

RTコンポーネント

Pythonファイルの設定

RTシステム構築

シミュレーション実行

RTCの仕様

TankJoystickControllerRTC_Py

JoystickPySDL2

choreonoid用openrtm連携プラグイン python版チュートリアル(tankjoystick)