Choreonoid用OpenRTM連携プラグイン Python版 チュートリアル(TankJoystick)

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


choreonoid-openrtm-py60.png

RTC作成

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

RTC Builder起動

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

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

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

RTCひな型作成

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


8b893cd4-2b7f-11e7-958c-8ba2052c448d.png

コンポーネント名称 TankIoRTC_Py
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名はモデルに対応するアイテムを選択後、左下のビューから""リンク""タブを表示すれば確認できます。


choreonoid-openrtm-py33.png

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シミュレータを選択して追加してください。 ※表示されるアイテムの順番が変わることがあるため、この画像と違う画面になる可能性があります。


choreonoid-openrtm-py31.png

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

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

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

モデル

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

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

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


cf26b56c-2b79-11e7-9cb9-7c3844c4508b.png

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

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

RTコンポーネント

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


choreonoid-openrtm-py32.png

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という項目を設定してください。


2c301ca2-2b7b-11e7-8ca8-25a7a17a73c1.png

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

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

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

RTシステム構築

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


cnoid-rtm-py6.png

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

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


cnoid-rtm-py7.png

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


cnoid-rtm-py8.png

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


5d02cf18-2b7c-11e7-8221-3e60171d65fd.png

シミュレーション実行

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

RTCの仕様

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

TankJoystickControllerRTC_Py

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


8e0862be-2b7f-11e7-9a44-b553218fe3c9.png

コンポーネント名称 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
説明 シミュレーションのステップ時間
Configuration
パラメーター名 KP
型 double
デフォルト値 200.0
説明 比例ゲイン
Configuration
パラメーター名 KD
型 double
デフォルト値 50.0
説明 微分ゲイン

JoystickPySDL2

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


8fa43300-2b7f-11e7-9fed-0deb4988cfdd.png

コンポーネント名称 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