このページではChoreonoid OpenRTM連携プラグイン Python版でTankモデルをゲームパッドで操作するまでの手順を説明します。 使用するモデル、作成するRTCはChoreonoid公式ページのチュートリアルとほぼ同じです。
この章ではシミュレータ上のアクチュエータ、センサなどの入出力を行うRTCの開発手順を説明します。
まずはRTC Builderでソースコードのひな型を作成します。 RTC Builder利用のために、OpenRTM-aistをインストールしてください。
インストールが完了したらRTC Builderを起動してひな型を作成してください。 Windows 8.1の場合は「スタート」>「アプリビュー(右下矢印)」>「OpenRTM-aist 2.0.1」>「OpenRTP」をクリックすると起動できます。
作成手順は以下が参考になります。
作成するRTCの仕様は以下のようになっています。言語はPythonを選択してください。
コンポーネント名称 | 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()
Link名はモデルに対応するアイテムを選択後、左下のビューから""リンク""タブを表示すれば確認できます。
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]
Windowsの場合はChoreonoidを展開したフォルダのbin/chorenoid.exeをダブルクリックしてください。
Ubuntuの場合はコマンドからchoreonoidと入力してください。
まずはワールドアイテム、シミュレータアイテムを追加します。 ファイル、新規からワールドとAISTシミュレータを選択して追加してください。 ※表示されるアイテムの順番が変わることがあるため、この画像と違う画面になる可能性があります。
World(ワールドアイテム) |-AISTSimulator(AISTシミュレータ)
アイテムツリーの概要、アイテムの移動方法は以下のページを参考にしてください。
次に環境、タンクのモデルを追加します。
ファイル、読み込みからOpenHRP モデルファイルを選択後、以下のファイルを読み込んでください。
World(ワールドアイテム) |-AISTSimulator(AISTシミュレータ) |-Tank(model/tank/tank.wrl) |-Labo1(model/Labo1/Labo1.wrl)
RTコンポーネントを追加します。 ファイル、新規からPyRTCを選択して追加してください。
この時点でアイテムツリーは以下のようになります。
World(ワールドアイテム) |-AISTSimulator(AISTシミュレータ) |-Tank(model/tank/tank.wrl) |-TankIO(PyRTC) |-Controller(PyRTC) |-Joystick(PyRTC) |-Labo1(model/Labo1/Labo1.wrl)
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システムを選択して追加してください。
'RTCリストが表示されていない場合は同様に表示してください。
シミュレーションを開始する前に、時間分解能を1000fpsに設定してください。 これでシミュレーションを開始するとゲームパッドのジョイスティックでシミュレータ上のクローラー、アームの操作、ボタンでライトのオンオフを操作できるようになります。
基本はChoreonoid付属のOpenRTMプラグインのサンプルと同じですが、一部データ型の変更、コンフィギュレーションパラメータの追加を行っています。
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 |
説明 | シミュレーションのステップ時間 |
Configuration | |
パラメーター名 | KP |
型 | double |
デフォルト値 | 200.0 |
説明 | 比例ゲイン |
Configuration | |
パラメーター名 | KD |
型 | double |
デフォルト値 | 50.0 |
説明 | 微分ゲイン |
ゲームパッドのアナログスティック、ボタン等の状態を出力する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 |