• はじめに
• Choreonoidの起動
• アイテムの追加
  • ワールド追加
  • シミュレータ追加
  • 地面追加
  • Raspberry Piマウス追加
  • RTSystem追加
  • RobotControllerコンポーネント追加
  • RaspberryPiMouseIoコンポーネント追加
• RaspberryPiMouseIoコンポーネント作成
  • RaspberryPiMouseIo.pyの編集
  • RaspberryPiMouseIoコンポーネントの設定
• アイテムの位置関係を確認
• ポート接続
• RobotControllerコンポーネントのアクティブ化
• シミュレーション開始
  • コンフィギュレーションパラメータの編集

はじめに

Choreonoidはオープンソースのロボット用シミュレーションソフトウェアです。 拡張性が高く、物理エンジン、通信機能、スクリプティング機能、制御アルゴリズム等をC++プラグインとして追加できます。

• Choreonoid ホームページ

このページでは、Choreonoidシミュレータ上の移動ロボットの入出力を行うRTCの作成手順を説明します。

Choreonoidの起動

講習会の実習の場合はUSBメモリ等で資料を配布しているため、USBメモリ内のchoreonoidフォルダ内のchoreonoid.batをダブルクリックして起動してください。

アイテムの追加

Choreonoid上で以下のアイテムを追加してシミュレーション環境を構築します。

 World(ワールドアイテム)
   |-AISTSimulator(AISTシミュレータ)
   |-Floor(model/misc/floor.body)
   |-RaspberryPiMouse(model/RaspberryPiMouse/RaspberryPiMouse.body)
       |-RaspberryPiMouseIo(PyRTC)
   |-RobotController(RTC)
   |-RTSystem

ワールド追加

Choreonoidで仮想世界を表現するワールドアイテムを追加します。 ボディモデル等の各アイテムはワールドアイテムと関連付けする必要があります。

「ファイル」->「新規」->「ワールド」をクリックしてWorldを追加してください。

名前は変更せずに生成します。

ワールドアイテムが追加されると、アイテムビューにWorldが表示されます。

シミュレータ追加

Choreonoidは複数のプラグインから使用する物理エンジンを選択することができます。 Choreonoid本体でサポートしているプラグインとしては、AISTシミュレータ、ODE, Bullet, PhysXが使用できます。

今回はAISTシミュレータを追加するため、「ファイル」->「新規」->「AISTシミュレータ」をクリックしてください。

名前は変更せずに生成します。

AISTシミュレータアイテムが追加されると、アイテムビューにAISTSimulatorが表示されます。

地面追加

地面を表現するボディアイテムを追加します。 「ファイル」->「読み込み」->「ボディ」をクリックしてください。

「ボディ読み込み」の画面でfloor.bodyを選択します。 左側のshareをクリックして、share/model/misc/floor.bodyのファイルを選択してください。

アイテムが追加されると、アイテムビューにFloorが表示されます。

Raspberry Piマウス追加

Raspberry Piマウスを表現するボディアイテムを追加します。 先ほどと同じ手順で「ファイル」->「読み込み」->「ボディ」をクリックしてください。

「ボディ読み込み」の画面でRaspberryPiMouse.bodyを選択します。 左側のshareをクリックして、share/model/RaspberryPiMouse/RaspberryPiMouse.bodyのファイルを選択してください。

アイテムが追加されると、アイテムビューにRaspberryPiMouseが表示されます。

また、シーンビューにRaspberryPiマウスの3Dモデルが表示されます。

RTSystem追加

Choreonoid上でRTSystemEditorの一部機能を使用するためのRTSystemアイテムを追加します。 「ファイル」->「新規」->「RTSystem」をクリックしてください。名前の変更は不要です。

アイテムが追加されると、アイテムビューにRTSystemが表示されます。

ネームサーバー、システムエディタ表示

この時点でChoreonoid上にネームサーバー、システムエディタは表示されていません。

まずネームサーバーを表示するには、「表示」->「ビューの表示」->「RTC List」をクリックします。

これでプロパティ、リンクプロパティの右にRTC Listタブが表示されます。

システムエディタを表示するには、「表示」->「ビューの表示」->「RTC Diagram」をクリックします。

これでシーンの右にRTC Diagramタブが表示されます。

RobotControllerコンポーネント追加

以下のページで作成したRobotControllerコンポーネントをシミュレータで利用可能にします。

• チュートリアル(RTコンポーネントの作成入門、Raspberry Pi Mouse、Windows)
• チュートリアル(RTコンポーネントの作成入門、Raspberry Pi Mouse、Ubuntu)

RTCを表現するRTCアイテムを追加してください。

「ファイル」->「新規」->「RTC」をクリックします。

名前をRobotControllerに変更します。

アイテムが追加されると、アイテムビューにRobotControllerが表示されます。

RobotControllerコンポーネントの設定

次にRobotControllerComp.exeとRobotControllerアイテムを関連付けます。 アイテムビューからRobotControllerを選択して、プロパティからRTC moduleを設定します。

「ファイルを選択」の画面でRobotControllerComp.exeを選択します。 初期の状態だと拡張子dllのファイルしか表示しないため、ファイルの種類を全てのファイル (*) に設定してからファイルを選択してください。

RaspberryPiMouseIoコンポーネント追加

PythonのRTCをChoreonoid上で起動するため、PyRTCアイテムを追加します。

「ファイル」->「新規」->「PyRTC」をクリックします。

名前をRaspberryPiMouseIoに変更します。

アイテムが追加されると、アイテムビューにRaspberryPiMouseIoが表示されます。

RaspberryPiMouseIoコンポーネント作成

ここからはRTC Builderで作業します。

以下の仕様のRTCを作成してください。

RaspberryPiMouseIo.pyの編集

コード生成すると、RaspberryPiMouseIo.pyが作成されるのでこのファイルをVisual Studio CodeやIDLEなどで編集します。

RaspberryPiMouseIoクラスにsetBody、outputToSimulator、inputFromSimulatorのメンバ関数を追加します。

setBody関数はRTC側でChoreonoidのボディオブジェクトを取得する関数です。 取得したBodyオブジェクトからLinkオブジェクトを取得することで、対象のJointの入出力ができます。

outputToSimulator関数は、シミュレータ上のオブジェクトから取得したデータをOutPortから出力する処理を行う関数です。

inputFromSimulator関数は、InPortの入力データをシミュレータ上のオブジェクトに入力する処理を行う関数です。

各関数は、シミュレーション実行時に以下の図のような順序で呼ばれます。

RaspberryPiMouseIo.pyに以下のコードを追加してください。 Pythonなので、インデントには注意してください。「def ～」を「# def onRateChanged～」のインデントに合わせる必要があります。

    # def onRateChanged(self, ec_id):
    #
    #    return RTC.RTC_OK
    
    def setBody(self, body):
        self.ioBody = body
        self.wheelR = self.ioBody.link("RIGHT_WHEEL")
        self.wheelL = self.ioBody.link("LEFT_WHEEL")
    
    def outputToSimulator(self):
        pass
    
    def inputFromSimulator(self):
        if self._velocityIn.isNew():
            data = self._velocityIn.read()
            
            vx = data.data.vx
            va = data.data.va
            
            wheel_distance = 0.0425
            wheel_radius = 0.04
            rms = (vx + va*wheel_distance)/wheel_radius
            lms = (vx - va*wheel_distance)/wheel_radius
            
            self.wheelR.dq = rms
            self.wheelL.dq = lms

RaspberryPiMouseIoコンポーネントの設定

Choreonoidでの作業に戻ります。 アイテムビューからRaspberryPiMouseIoを選択して、プロパティからRTC Moduleを設定します。

「ファイルを選択」の画面で先ほど編集したRaspberryPiMouseIo.pyを選択します。

※RaspberryPiMouseIo.pyを更新した場合は、再度この作業を行う事で再読み込みしてください。

アイテムの位置関係を確認

アイテムビューで、全てのアイテムはワールドアイテムの子アイテムとして配置する必要があります。 また、RaspberryPiMouseIoはRaspberryPiMouseの子アイテムとして配置します。

 World
     |- AISTSimulator
     |- Floor
     |- RaspberryPiMouse
     |     |- RaspberryPiMouseIo
     |- RobotController
     |- RTSystem

位置関係が違う場合はドラッグアンドドロップして移動してください。

ポート接続

起動したRTCのポートを接続します。

この時点でRobotController0とRaspberryPiMouseIo0が起動していますが、RTC Listに表示が無い場合は「Update」ボタンを押してください。

RTC ListからRTCをRTC Diagramにドラッグアンドドロップして、以下のポートを接続してください。

RobotControllerコンポーネントのアクティブ化

RTCアイテムにexeファイルを設定した場合はRTCを自動でアクティブ化しないため、RTC Diagram上でRobotController0を右クリックして「Activate」を選択してください。 ※RaspberryPiMouseIo0はシミュレーションを開始すると自動的にアクティブ状態になるため操作の必要はありません。

シミュレーション開始

「初期位置からのシミュレーション開始」ボタンを押すとシミュレーションを開始します。

シーンタブに切り替えるとシミュレーションを3DCGで表示します。

コンフィギュレーションパラメータの編集

Choreonoid OpenRTMプラグインにはコンフィギュレーションパラメータ編集機能がないため、RTSystemEditorで作業します。

ネームサービスビューでRobotControllerを選択して、コンフィギュレーションビューから「編集」ボタンを押して、コンフィギュレーションパラメータを変更してみてください。

Choreonoidで表示したRaspberry Piマウスが移動しているか確認すれば完了です。

ネームサービスビューにlocalhostと表示されていない場合については、以下のようにネームサービス接続ボタンを押して、localhostのネームサーバーに接続してください。

• はじめに
• ArmControllerコンポーネントの作成
  • ロボットアーム共通インターフェース
  • ソースコードの編集とビルド
• シミュレーション
  • Choreonoid起動
  • RTCItem追加
  • シミュレーション実行
• myCobot実機の制御
  • myCobot280の準備
  • RTC起動

はじめに

Choreonoid上のmyCobot280の制御コンポーネントの作成手順、及びmyCobot280実機の制御システムの構築手順を説明します。

ArmControllerコンポーネントの作成

myCobot280制御コンポーネントにコマンドを送信するRTCを作成します。 RTC Builderで以下の仕様のRTCを作成してください。

ロボットアーム共通インターフェース

ロボットアームの制御のため、ロボットアーム共通インターフェースのコマンドを使用する。

• ロボットアーム共通インターフェース

サービスポートの設定

サービスポートの設定手順について説明します。 「サービスポート」タブを選択し、サービスポートの情報を入力します。

「Add Port」ボタンを押下してサービスポートを追加後、名前を設定します。

サービスポートに「Add Interface」ボタンを押下後、各項目を設定します。

各情報は以下の通り設定してください。

• ServicePort Profile:
  • ポート名: middle
  • サービスインターフェース:
    • インターフェース名:JARA_ARM_ManipulatorCommonInterface_Middle
    • 方向:Required
    • インターフェース型: JARA_ARM::ManipulatorCommonInterface_Middle

• ServicePort Profile:
  • ポート名: common
  • サービスインターフェース:
    • インターフェース名:JARA_ARM_ManipulatorCommonInterface_Common
    • 方向:Required
    • インターフェース型:JARA_ARM::ManipulatorCommonInterface_Common

コード生成時に以下の画面が表示されますが、「はい」を選択してください。

ソースコードの編集とビルド

ソースコード (src/ArmController.cpp) を編集します。

下記のように、onActivated()、onDeactivated()、onExecute() を実装します。

 RTC::ReturnCode_t ArmController::onActivated(RTC::UniqueId /*ec_id*/)
 {
    //サーボをONにするコマンドを実行
    m_JARA_ARM_ManipulatorCommonInterface_Common->servoON();
    //アームを原点復帰位置に移動する
    m_JARA_ARM_ManipulatorCommonInterface_Middle->goHome();
  return RTC::RTC_OK;
 }

 RTC::ReturnCode_t ArmController::onDeactivated(RTC::UniqueId /*ec_id*/)
 {
    //サーボをOFFにするコマンドを実行
    m_JARA_ARM_ManipulatorCommonInterface_Common->servoOFF();
  return RTC::RTC_OK;
 }

 RTC::ReturnCode_t ArmController::onExecute(RTC::UniqueId /*ec_id*/)
 {
    //コンフィギュレーションパラメータの変更時のみ実行
    if (getConfigService().isChanged())
    {
        //コンフィギュレーションパラメータを更新する
        updateParameters("default");
        //CarPosWithElbow型データに手先の位置姿勢を格納する
        JARA_ARM::CarPosWithElbow pose;
        //姿勢は固定
        pose.carPos[0][0] = -1;
        pose.carPos[0][1] = 0;
        pose.carPos[0][2] = 0;
        pose.carPos[1][0] = 0;
        pose.carPos[1][1] = 1;
        pose.carPos[1][2] = 0;
        pose.carPos[2][0] = 0;
        pose.carPos[2][1] = 0;
        pose.carPos[2][2] = -1;
        //コンフィギュレーションパラメータを位置情報として格納
        pose.carPos[0][3] = m_pos_x;
        pose.carPos[1][3] = m_pos_y;
        pose.carPos[2][3] = m_pos_z;
        pose.elbow = 0;
        pose.structFlag = 0;
        //絶対値で指定した目標位置に移動するコマンドを実行
        //移動完了まで処理は戻らない
        m_JARA_ARM_ManipulatorCommonInterface_Middle->moveLinearCartesianAbs(pose);
    }
 
  return RTC::RTC_OK;
 }

Visual Studioでビルドすると実行ファイル(build/src/Release(or Debug)/ArmControllerComp.exe)が生成されます。

シミュレーション

Choreonoid起動

講習会資料に含まれているchoreonoidフォルダ内のmyCobot280Simulator.batをダブルクリックして起動してください。

• RTM_Tutorial.zip

Ubuntuの場合は以下のコマンドで起動します。

 choreonoid ${資料のディレクトリ}/choreonoid/myCobot280Sample/myCobot280Sample.cnoid

RTCItem追加

ArmControllerを起動するため、Choreonoid上でRTCItemを生成します。 「ファイル」→「新規」→「RTC」をクリックしてください。

名前は「ArmController」に設定して生成してください。

生成したArmControllerアイテムのプロパティから、RTCモジュールのパスを設定します。

ファイル選択のダイアログでは、File of typeを「全てのファイル(*)」を設定してください。 その後、プロジェクトののbuildフォルダからArmControllerComp.exeを選択してください。 ※Ubuntuの場合はArmController.soを選択してください。

次にRTCリストからRTCダイアグラムにmyCobot280Io0とArmController0をドラッグアンドドロップしてください。 RTCの一覧が見えない場合は「更新」ボタンを押してください。

RTCダイアグラム上でmyCobot280Io0とArmController0の「middle」ポート同士と「common」ポート同士を接続します。

アイテムのツリー構造が以下のようになっているか確認してください。

 World(ワールドアイテム)
   |-myCobot(Body)
       |-myCobot280Io(PyRTC)
   |-AISTSimulator(AISTシミュレータ)
   |-myCobot280Sample(RTSystem)
   |-ArmController(RTC)

シミュレーション実行

以下のボタンを押してシミュレーションを開始してください。

RTCリストから「ArmController0」をクリックするとコンフィギュレーションビューからパラメータの操作ができます。 からpos_x、pos_y、pos_zの値をクリックして変更してください。「適用」を押したタイミングでコマンドが送信されてアームの手先位置が制御されます。

myCobot実機の制御

myCobot280の準備

myCobot280をACアダプタ、USBケーブルを接続し、USBケーブルはPCに接続してください。

myCobotの操作画面から、「Transponder」を選択して「myCobnot Basic Transponder」の画面を表示してください。

接続後、デバイスマネージャからポート番号(COM**)を確認しておいてください。

RTC起動

ArmControllerの起動

ArmControllerComp.exeと同じフォルダに以下の内容のrtc.confを作成してください。 コマンドで手先位置移動完了まで処理を返さないため、通信がタイムアウトしないように設定します。

 corba.args: -ORBclientCallTimeOutPeriod 0

ファイル構成は以下のようになります。

 ArmController
     |- build
           |- src
                |- Release(Debug)
                           |- ArmControllerComp.exe
                           |- rtc.conf

ArmControllerComp.exeをダブルクリックして起動してください。 Ubuntuの場合はArmControllerCompを起動します。

myCobotRTCの起動

講習会資料に含まれているchoreonoidフォルダ内のmyCobotRTCフォルダ内のmyCobot.confを編集します。 以下の「conf.default.com_port」でデバイスマネージャで確認したポート番号(COM**)を指定してください。

 conf.default.com_port: COM11

choreonoidフォルダ内のmyCobot280RTC.batをダブルクリックして起動してください。 Ubuntuの場合は以下のコマンドで起動します。

 python3 myCobot.py

動作確認

RTシステムエディタで「ArmController0」と「myCobot0」の「middle」ポート同士と「common」ポート同士で接続してアクティブ化してください。

コンフィギュレーションパラメータを変更後に編集ボタンを押すとmyCobot280の手先位置が制御されます。