コントローラ作成ガイド(歩行パターン)(Ver.3.0.7)

概要

歩行パターンファイルに基づきPD制御によってロボットを歩行させるSamplePDを例にして、OpenRTMコンポーネントとしてコントローラを作成する方法を説明します。
OpenHRPソースパッケージに用意されているサンプルコード(SamplePD.h、SamplePD.cpp)を参照しながらコンポーネントを作成する過程を追っていきます。
説明で省いた部分のコードはrtc-templateが作成した雛形に相当する部分となります。
rtc-templateを用いたOpenRTMコンポーネントの雛形作成については「スケルトンの作成」にて後述いたします。

コード解説

SamplePD.h

Controller/rtc/SamplePD/SamplePD.h をご覧ください。

SamplePD::onActivated と SamplePD::onExecute が宣言されていることを確認してください。 雛形作成時には SamplePD::onInitialize 以外の仮想関数がコメントアウトされています。 これらの仮想関数をオーバーライドすることでコントローラ特有の振る舞いを定義することができます。

コントローラが使用する種々のメンバが追加されています。

private: 
  int dummy; 
  std::ifstream angle, vel, gain; // 関節角度、 関節角速度、ゲイン値
  double *Pgain;                  // Pゲインの配列
  double *Dgain;                  // Dゲインの配列
  std::vector<double> qold;       // ひとつ前の関節角度を保持

Windows環境でも実行する場合は、DLL版のビルドに対応するためSamplePDInitメソッドをOpenRTMで定義されたDLL_EXPORTマクロで修飾します。

extern "C"
{

  DLL_EXPORT void SamplePDInit(RTC::Manager* manager);

};

SamplePD.cpp

ヘッダファイルのインクルードとマクロの定義を行います。

#include <iostream>

#define DOF (29)       // 自由度
#define TIMESTEP 0.002 // シミュレーションのステップ

// 各種ファイル群
#define ANGLE_FILE "etc/angle.dat"  // 関節角度
#define VEL_FILE   "etc/vel.dat"    // 関節角速度
#define GAIN_FILE  "etc/PDgain.dat" // PDゲイン値

SamplePD.hで宣言したメソッドを実装します。

SamplePD::SamplePDでコンストラクタを呼んだ後、歩行パターンファイルを開き、ゲインファイルからPDゲイン値を変数に取り込みます。

SamplePD::SamplePD(RTC::Manager* manager)
  : RTC::DataFlowComponentBase(manager),
    // <rtc-template block="initializer">
    m_angleIn("angle", m_angle),
    m_torqueOut("torque", m_torque),
    
    // </rtc-template>
    dummy(0),
    qold(DOF)
{

...

  Pgain = new double[DOF];
  Dgain = new double[DOF];

  // 関節角度ファイルを開く
  if (access(ANGLE_FILE, 0)){
    std::cerr << ANGLE_FILE << " not found" << std::endl;
  }else{
    angle.open(ANGLE_FILE);
  }

  // 関節角速度ファイルを開く
  if (access(VEL_FILE, 0)){
    std::cerr << VEL_FILE << " not found" << std::endl;
  }else{
    vel.open(VEL_FILE);
  }

  // ゲインファイルを開き配列に代入
  if (access(GAIN_FILE, 0)){
    std::cerr << GAIN_FILE << " not found" << std::endl;
  }else{
    gain.open(GAIN_FILE);
    for (int i=0; i<DOF; i++){
      gain >> Pgain[i];
      gain >> Dgain[i];
    }
    gain.close();
  }

  // トルク, 関節角度ポートの長さをロボットの自由度分確保
  m_torque.data.length(DOF);
  m_angle.data.length(DOF);
}

SamplePD::~SamplePDでファイルのクローズと配列の解放を行います。

  if (angle.is_open()) angle.close();
  if (vel.is_open()) vel.close();
  delete [] Pgain;
  delete [] Dgain;

RTC::ReturnCode_t SamplePD::onActivatedでは初期化処理を行います。

RTC::ReturnCode_t SamplePD::onActivated(RTC::UniqueId ec_id)
{
  std::cout << "on Activated" << std::endl;
  angle.seekg(0);
  vel.seekg(0);

  // 関節角度InPortの値をアップデート
  m_angleIn.update();

  // １フレーム前の値を保持する
  for(int i=0; i < DOF; ++i){
    qold[i] = m_angle.data[i];
  }

  return RTC::RTC_OK;
}

RTC::ReturnCode_t SamplePD::onExecuteメソッドでシミュレーションのステップ毎の振る舞いを処理します。

RTC::ReturnCode_t SamplePD::onExecute(RTC::UniqueId ec_id)
{
  // 関節角度InPortの値をアップデート
  m_angleIn.update();

  // 各関節の角度と角速度を更新
  static double q_ref[DOF], dq_ref[DOF];
  if(!angle.eof()){
    angle >> q_ref[0]; vel >> dq_ref[0];// skip time
    for (int i=0; i<DOF; i++){
      angle >> q_ref[i];
      vel >> dq_ref[i];
    }
  }
  
  // 各関節のtorque値を決定
  for(int i=0; i<DOF; i++){
    double q = m_angle.data[i];
    double dq = (q - qold[i]) / TIMESTEP;
    qold[i] = q;
    
    m_torque.data[i] = -(q - q_ref[i]) * Pgain[i] - (dq - dq_ref[i]) * Dgain[i];
  }  
  
  //トルクを出力
  m_torqueOut.write();
  
  return RTC::RTC_OK;
}

ファイル

Controller/rtc/SamplePD/以下のパターンファイルと設定ファイルについて説明します。

歩行パターンファイル

コードでいうところのANGLE_FILEマクロ、VEL_FILEマクロに相当するファイルです。
歩行パターンファイルのフォーマットについて説明します。以下が一行分のファイルのフォーマットです。

時刻  <JointID=0 の関節のデータ>  <JointID=1 の関節のデータ>  …  <JointID=nの関節のデータ>

一行が１フレームに相当し、デリミッタはtabです。
時刻は開始時刻からの相対時間、関節のデータはangle.datの場合角度、vel.datの場合角速度に相当します。
例えばサンプルの場合、14秒間6701フレームの29個の関節データが表現されています。

ゲインファイル

コードでいうところのGAIN_FILEマクロに相当するファイルです。
PD制御のゲインを記録します。
そのフォーマットはJointID行にPゲイン、Dゲインの任意個をスペースでわけて書くこととし、具体的には以下のとおりとします。

Pゲイン Dゲイン (<= JointID = 0)
Pゲイン Dゲイン (<= JointID = 1)
...
Pゲイン Dゲイン (<= JointID = n)

rtc.conf

設定ファイル Controller/rtc/SamplePD/rtc.conf をご覧ください。
RTコンポーネントとしての設定が記述されています。

環境に合わせてネームサーバの場所を記述します。

corba.nameservers: localhost:2809

特に,

Make.vars, bin/unix/config.sh              (Unix 環境)
OpenHRP.vsprops, bin/dos/config.bat   (Windows 環境)

のNS_HOST、NS_PORTがデフォルト値で無い場合は変更にあわせる必要があります。

ログファイル作りの有無を設定します。

• ログファイル作りを無効にする場合

  logger.enable:NO

• ログファイル作りを有効にする場合

  logger.enable:YES

ログファイルの作り先（パス）と名前形式を設定します。
以下の例のようにパスを直接記述することも可能です。

logger.file_name: D:\\Temp\\rtc%p.log

ログレベルの設定です。

logger.log_level: TRACE

以下の内容は基本的には変更しないでください。

naming.formats: %n.rtc
manager.modules.load_path: .
exec_cxt.periodic.rate: 1000000
manager.modules.abs_path_allowed: yes
exec_cxt.periodic.type: SynchExtTriggerEC

RTコンポーネントの設定オプションに関する詳しい説明は、OpenRTM マニュアルの コンフィギュレーション で確認してください。

コントローラブリッジの設定と起動

各コンポーネント毎に実装を分離する目的は、開発における保守性・可搬性の向上です。
コントローラブリッジは、シミュレーション対象のモデルと各OpenRTMコンポーネント間の入出力を介するプロセスになります。
ここでは、コントローラブリッジの起動とコンポーネントとの接続設定を説明します。

Linuxの場合

起動スクリプトController/rtc/SamplePD/SamplePD.shをご覧ください。

#!/bin/sh

CONTROLLER_BRIDGE_DIR=../../bridge
if [ -z $NS_HOST ]; then
    if [ -z $NS_PORT ]; then
        . $CONTROLLER_BRIDGE_DIR/../../bin/unix/config.sh
    fi
fi
$CONTROLLER_BRIDGE_DIR/ControllerBridge \
--config-file bridge.conf \
--name-server $NS_HOST:$NS_PORT

Controller/bridgeディレクトリとController/SamplePDディレクトリの相対位置が変わった場合、 CONTROLLER_BRIDGE_DIRを適宜に変更することでControllerBridgeへの参照が対応するように なっています。
環境変数 NS_HOST、NS_PORTが定義されていない場合、bin/unix/config.shより取得するようになっています。

Windowsの場合

起動スクリプトController/rtc/SamplePD/SamplePD.batをご覧ください。

SET CONTROLLER_BRIDGE_DIR=..\..\bridge
@echo off
if "%NS_HOST%" == "" (
    if "%NS_PORT%" == "" (
        call %CONTROLLER_BRIDGE_DIR%\..\..\bin\dos\config.bat
    )
)
@echo on

%CONTROLLER_BRIDGE_DIR%\ControllerBridge ^
--config-file bridge.conf ^
--name-server %NS_HOST%:%NS_PORT%

SamplePD.shと同様にController/bridgeディレクトリとController/SamplePDディレクトリの相対位置が変わった場合、 ControllerBridgeが参照できるようにCONTROLLER_BRIDGE_DIRの変更によって対応するようになっております。
パス名にスペースが入る際はダブルクォテーションで囲ってください。
環境変数 NS_HOST、NS_PORTが定義されていない場合、bin/dos/config.batより取得できるようにします。
Windows環境における当バッチファイルの実行において注意点があります。
それは、Linux環境と違いomninamesサーバをサービスとして登録しない場合が多いので、そのようなときは事前にomninamesサーバを起動しておく必要があります。
マニュアルによるomninamesサーバの起動方法はRTコンポーネント作成(VC++版)「ネームサーバの起動」項目を参照してください。
もしくは、事前にGrxUIを起動していただければGrxUI動作中は、omninamesサーバが動作します。
ただしGrxUIによるomninamesサーバの起動は、GrxUIを終了するとomninamesサーバも終了しますので注意が必要です。

起動オプションについて

config-file
起動オプションを設定したファイルを指定します。
上述では、bridge.conf設定ファイルを使用します。

name-server
ネームサーバーのホストとポート番号を指定します。
上述では、ホスト名に環境変数NS_HOSTをポート番号に環境変数NS_PORTを割り当てます。

起動オプションの詳細、その他の起動オプションについては「コントローラーブリッジ使用マニュアル」をご覧ください。

プロジェクトファイルの読み込みと実行

GrxUIからSamplePD.xmlを読み込み動作を確認します。

1. メニューの File→Load Project を選択してOpen Project File ダイアログからSamplePD.xmlを読み込みます。
2. 「start simulation」ボタンを押下します。


図1 : start simulation」ボタン

3. 図2 ダイアログのNoボタンを押下します。


図2 : 「Restart the Controller」 ダイアログ

GrxUIの詳細については「GrxUI」をご覧ください。

スケルトンの作成

コンポーネントのスケルトン作成について説明します。 コマンドライン上でも作成できますが設定項目が多いので、スクリプト用いた作成手法を環境毎に説明します。

このスクリプトの実行によってangleという名前のInPort、torqueという名前のOutPortを備えたSamplePDコンポーネントのスケルトンを生成します。

Linuxの場合

以下のようなシェルスクリプトを用意します。

#!/bin/sh

rtc-template -bcxx \
--module-name=SamplePD --module-desc="SamplePD component" \
--module-version=0.1 --module-vendor=AIST --module-category=Generic \
--module-comp-type=DataFlowComponent --module-act-type=SPORADIC \
--module-max-inst=1  \
--inport=angle:TimedDoubleSeq \
--outport=torque:TimedDoubleSeq

rtc-template(Linux C++版)に関する詳しい説明は、OpenRTM マニュアルの RTコンポーネント作成 で確認してください。

Windowsの場合

以下のようなバッチを用意します。

"C:\Program Files\OpenRTM-aist\0.4\utils\rtc-template\rtc-template.py" -bcxx ^
--module-name=SamplePD --module-desc="SamplePD component" ^
--module-version=0.1 --module-vendor=AIST --module-category=Generic ^
--module-comp-type=DataFlowComponent --module-act-type=SPORADIC ^
--module-max-inst=1 ^
--inport=angle:TimedDoubleSeq ^
--outport=torque:TimedDoubleSeq

バッチの実行によって、VC++用のソリューションファイル、VC++ 2008用のプロジェクトファイルを生成します。 実行後、rtc-templateが自動生成した copyprops.bat を実行してください。

rtc-template(Windows VC++版)に関する詳しい説明は、OpenRTM マニュアルの RTコンポーネント作成(VC++版) で確認してください。