ステップ2: コントローラの導入¶

ステップ1ではコントローラが無かったため、シミュレーションの際にTankモデルの砲身の姿勢を維持することができませんでした。そこでステップ2では姿勢を維持するための最小限のコントローラの作成を通して、コントローラ導入の基本を学ぶことにします。

コントローラの形式について
コントローラ "TurretController1" の実装
プロジェクトディレクトリの作成
コントローラのビルド方法
CMakeLists.txtの記述
コントローラのコンパイル
コントローラアイテムの生成
コントローラ本体のセット
シミュレーションの実行
実装内容の解説

コントローラの形式について ¶

一般的に、コントローラの実装形式は様々なものがあり得ます。特定のロボットシステムやシミュレータが規定する形式もありますし、OpenRTMやROSといった汎用的なミドルウェアの規定する形式でコントローラを実装することも一般的となっています。

これに関して、本チュートリアルでは、Choreonoid独自の「シンプルコントローラ（SimpleController）」の形式でコントローラの実装を行います。シンプルコントローラは、C++言語とChoreonoid内部のデータ構造を用いてコントローラを実装するもので、OpenRTMやROSと比べて覚えることが少なくて済み、コードも比較的シンプルなものになるといった利点があります。

ただしこれはChoreonoidの独自形式であるため、汎用性の面ではOpenRTMやROSに劣ります。また、OpenRTMやROSが提供するような通信機能を提供するものでもありません。ChoreonoidはOpenRTMやROSと連携する機能も備えていますので、必要に応じてそれらの機能を用いるようにして下さい。OpenRTMに関しては、 OpenRTM連携機能で解説しています。

コントローラ "TurretController1" の実装 ¶

シンプルコントローラ形式ではC++のクラスとしてコントローラを実装します。ここでは砲塔(Turret)のピッチ軸を維持するだけの "TurretController1" の実装を行います。このコントローラのソースコードを以下に示します。

#include <cnoid/SimpleController>

using namespace cnoid;

class TurretController1 : public SimpleController
{
    Link* joint;
    double q_ref;
    double q_prev;
    double dt;

public:
    virtual bool initialize(SimpleControllerIO* io) override
    {
        joint = io->body()->link("TURRET_P");
        joint->setActuationMode(Link::JOINT_TORQUE);
        io->enableIO(joint);
        q_ref = q_prev = joint->q();

        dt = io->timeStep();

        return true;
    }

    virtual bool control() override
    {
        // PD gains
        static const double P = 200.0;
        static const double D = 50.0;

        double q = joint->q(); // input
        double dq = (q - q_prev) / dt;
        double dq_ref = 0.0;
        joint->u() = P * (q_ref - q) + D * (dq_ref - dq); // output
        q_prev = q;

        return true;
    }
};

CNOID_IMPLEMENT_SIMPLE_CONTROLLER_FACTORY(TurretController1)

以下ではまずこのコントローラをシミュレーションプロジェクトに導入し、シミュレーションを行うまでを解説します。その後、コントローラの実装内容について解説したいと思います。

上記のソースコードはテキストエディタ等を用いて入力するとして、これを保存するためのディレクトリを作成しましょう。例えば "tank" というディレクトリを作成し、ここに上記のソースコードを "TurretController1.cpp" というファイル名で保存します。今後もこのチュートリアルに関するファイルはこのディレクトリに格納するものとし、これを 「プロジェクトディレクトリ」 と呼ぶことにします。

なお、ステップ1でプロジェクトの保存を行ったファイルについても、このディレクトリにまとめて保存するとよいでしょう。

注釈

Ubuntuでどのテキストエディタを使ったらよいか分からないという方は、とりあえず標準の "gedit" というテキストエディタを使ってみて下さい。Dashから"gedit"と入力すると出てくる「テキストエディター」というアイコンで起動できます。コマンドラインから "gedit" と入力してもOKです。

注釈

ソースコードのファイルはChoreonoid本体の "sample/tutorial/Tank/" 以下に格納されています。自分で入力するのが面倒な場合は、このファイルを利用してもOKです。本チュートリアルで言及する他のファイルについてもこのディレクトリに格納されていますので、必要に応じてご利用下さい。

コントローラのビルド方法 ¶

C++言語で記述されたコントローラのソースコードをビルド（コンパイル）するにあたって、主に以下の２通りの方法があります。

Choreonoid本体と一緒にビルドを行う
Choreonoid本体とは別にビルドを行う

Choreonoidをソースからビルドしている場合、1の方が手軽ですので、本チュートリアルでは1で解説を進めることにします。

ビルド方法の詳細についてはコントローラのビルドで解説していますのでそちらを参照してください。実際のコントローラ開発時には、環境や目的に応じて両手法を使い分けることになるかと思います。

1を採用する場合、先ほど作成したプロジェクトディレクトリをChoreonoid本体のビルドシステムに認識させる必要があります。これについても２通りの方法があります。

Choreonoidのソースディレクトリ内の "ext" ディレクトリに対象のディレクトリを配置する
ChoreonoidのCMakeの設定で "ADDITIONAL_EXT_DIRECTORIES" に対象のディレクトリを指定する

Aを採用する場合は、先ほど作成したプロジェクトディレクトリ "tank" を上記の ext ディレクトリ以下に移動して下さい。話が前後しますが、初めから ext 以下にディレクトリを作成しても、もちろんOKです。

Bを採用する場合は、対象のディレクトリへのパスを、上記の設定項目に指定します。対象ディレクトリが複数ある場合は、セミコロンで区切って入力します。

特に理由がなければAの方法を用いることにしましょう。この場合、

Choreonoidのソースディレクトリの "ext" ディレクトリ以下にプロジェクトディレクトリ "tank" を作成する
前節に掲載した"TurretController1"のソースコードを "TurretController1.cpp" というファイル名で "tank" ディレクトリに保存する

とすればOKです。

CMakeLists.txtの記述 ¶

次に、プロジェクトディレクトリに "CMakeLists.txt" というテキストファイルを作成し、ここにコントローラのコンパイルに関する設定を記述します。

といっても今回記述すべき内容は非常にシンプルで、以下の一行を記述すればOKです。

add_cnoid_simple_controller(TankTutorial_TurretController1 TurretController1.cpp)

ここで用いている "add_cnoid_simple_controller" という関数は、Choreonoid本体のCMake記述にて予め定義された関数です。この関数に、生成されるコントローラの名前とソースファイルを与えるだけで、コントローラのコンパイルを行うことができます。

今回コントローラ名に "TankTutorial" というプレフィックスをつけています。これは必須というわけではありませんが、今後他のプロジェクトで開発したコントローラと区別しやすくするためにつけています。

コントローラのコンパイル ¶

コンパイルを行いましょう。今回はChoreonoid本体と一緒にビルドする手法を用いているので、再度Choreonoid本体のビルドを行えばOKです。今回CMakeLists.txtが追加されましたので、まずそれを認識させるため、CMakeを再実行しましょう。Step1で開いた端末についてはChoreonoid本体のソースディレクトリがカレントディレクトリになっているかと思います。そうでない場合は

cd [Choreonoid本体のソースディレクトリへのパス]

として、Choreonoid本体のソースディレクトリに移動してください。

ソースディレクトリをそのままビルドディレクトリとしている場合は、この状態で

cmake .

としてCMakeを再実行します。ビルドディレクトリをソースディレクトリとは分けている場合は、ビルドディレクトリに移動して、cmakeのパラメータにソースディレクトリを指定します。例えば、ソースディレクトリ直下にbuildというビルドディレクトリを作成している場合は、

cd build
cmake ..

となるかと思います。

つぎにビルドディレクトリ上で続けて

make

と入力して下さい。（コンパイル方法の詳細はソースコードからのビルドとインストール (Ubuntu Linux編) の Choreonoidのビルドを参照して下さい。）

この際コントローラのビルド方法で述べたAかBの条件を満たしていれば、上記のCMakeLists.txtが検出され、その内容も実行されるというわけです。

コンパイルに成功すると、ビルドディレクトリの "lib/choreonoid-x.x/simplecontroller" （x.xはChoreonoidのバージョン番号）に、

TankTutorial_TurretController1.so

というファイルが生成されるはずです。これがコントローラ本体のファイルとなります。この拡張子からも分かるように、コントローラの実態は共有ライブラリファイルとなります。コントローラが生成されたディレクトリは今後 「コントローラディレクトリ」 と呼ぶことにします。

コンパイルエラーが出た場合は、エラーメッセージを参考にして、ソースコードやCMakeLists.txtの記述を見直してみて下さい。

コントローラアイテムの生成 ¶

作成したシンプルコントローラは、Choreonoid上で「シンプルコントローラアイテム」を用いて導入します。

そこでまずシンプルコントローラアイテムを生成しましょう。メインメニューの「ファイル」-「新規」から「シンプルコントローラ」を選択して生成します。アイテムの名前はなんでもよいですが、コントローラに合わせて "TurretController" とするとよいでしょう。

生成したアイテムは、下図のように、制御対象のTankアイテムの小アイテムとして配置するようにします。

この配置によって、コントローラの制御対象がTankモデルであることを明示します。これを実現するにあたっては、Tankアイテムを選択状態としてからコントローラアイテムの生成を行ってもよいですし、生成後にこの配置になるようドラッグしてもOKです。

コントローラ本体のセット ¶

次に先ほど作成したコントローラの本体をシンプルコントローラアイテムにセットします。

これはシンプルコントローラアイテムの「コントローラモジュール」というプロパティを用いて行います。まず、アイテムツリー上で "TurretController" を選択します。するとこのアイテムのプロパティ一覧がアイテムプロパティビュー上に表示されますので、その中から「コントローラモジュール」というプロパティを探して下さい。そのプロパティの値の部分（デフォルトでは空欄となっている）をダブルクリックすると、モジュールのファイルを入力することができます。

この際、入力用のファイルダイアログを用いて入力するのが手軽です。コントローラモジュールの入力時には下図に示すように値を入力する箇所の右端にアイコンがあります。

../../_images/controller-module-property.png

このアイコンをクリックすると、ファイル選択のダイアログが表示されます。このダイアログは通常シンプルコントローラ格納用の標準ディレクトリを指しています。そこには先ほど作成した "TankTutorial_TurretController1.so" が格納されているはずですので、これを選択して下さい。

これでコントローラ本体がシンプルコントローラアイテムにセットされました。これでコントローラを機能させることができます。

ここまで設定できたら、またプロジェクトを保存しておきましょう。ファイル名は "step2.cnoid" として、プロジェクトディレクトリに保存しておくとよいかと思います。

シミュレーションの実行 ¶

以上の設定を行った上でシミュレーションを実行して下さい。すると、ステップ1では重力で下を向いてしまった砲身が、今回は正面を向いたままとなっているはずです。これはコントローラ "TurretController1" によって、姿勢の維持に必要なトルクが砲塔ピッチ軸にかけられているからです。

うまくいかない場合は、メッセージビューも確認してみて下さい。コントローラの設定や稼働に問題があると、シミュレーション開始時にその旨を知らせるメッセージが出力される場合があります。

なお、このコントローラでは砲塔ヨー軸の制御は行っていないため、そちらには力がかかっていません。Step1の時と同様に、インタラクション機能を用いて砲塔部分をドラッグすると、ヨー軸に関してはフリーで動かせることが分かります。

実装内容の解説 ¶

今回作成したコントローラ "TurretController1" の実装内容は以下のようになっています。

SimpleControllerクラス ¶

まず、シンプルコントローラはChoreonoidで定義されている "SimpleController" クラスを継承したクラスとして実装するようになっています。そこでまず

#include <cnoid/SimpleController>

によって、このクラスが定義されているヘッダをインクルードしています。Choreonoidが提供するヘッダファイルはインクルードディレクトリの "cnoid" サブディレクトリに格納されるようになっており、このように cnoid ディレクトリからのパスとして記述します。拡張子は必要ありません。

また、Choreonoidで定義されているクラスは全て "cnoid" という名前空間に所属しています。ここでは

using namespace cnoid;

によって名前区間を省略できるようにしています。

コントローラのクラス定義は、

class TurretController1 : public SimpleController
{
    ...
};

によって行われています。SimpleControllerを継承するかたちでTurretController1を定義していることが分かります。

SimpleControllerクラスではいくつかの関数が仮想（バーチャル）関数として定義されており、継承先でそれらの関数をオーバーライドすることでコントローラの処理内容を実装します。通常以下の２つの関数をオーバーライドします。

virtual bool initialize(SimpleControllerIO* io)
virtual bool control()

initialize関数の実装 ¶

initialize関数はコントローラの初期化を行う関数で、シミュレーション開始の直前に１回だけ実行されます。

この関数に引数として与えられるSimpleControllerIO型は、コントローラの入出力に必要な機能をまとめたクラスとなっています。この詳細はコントローラの実装の IOオブジェクトをみていただくとして、ここではまず

joint = io->body()->link("TURRET_P");

によって、砲塔ピッチ軸の入出力を行うためのLinkオブジェクトを取得し、joint変数に格納しています。

io->body() によってTankモデル入出力用のBodyオブジェクトを取得し、続けてこのオブジェクトが有するLinkオブジェクトから "TURRET_P" という名前を持つものを取得しています。これは Tankモデルの作成において記述した砲塔ピッチ軸部の関節に対応するものです。

次に

joint->setActuationMode(Link::JOINT_TORQUE);

によって、この関節のアクチュエーションモードを関節トルクに設定しています。これにより、関節トルクを指令値とした制御が可能となります。

また、

io->enableIO(joint);

によって、この関節に対する入出力を有効にしています。この記述は関節のデフォルトの入出力を有効化するものです。今回悪チュエーションモードが関節トルクとなっているので、関節角度を入力し、関節トルクを出力することになります。これによってこの関節に対してPD制御を行うことが可能となります。

関節に対して上記のアクチュエーションモードの設定や入出力の有効化を行わない場合、その関節の制御を行うことはできませんので、ご注意下さい。入出力を設定する関数としては、他に入力飲みを設定する enableInput という関数と、出力のみを設定する enableOutput という関数も利用可能です。

注釈

同様のことを行う関数として、SimpleControllerIOのsetLinkInput、setJointInput、setLinkOutput、setJointOutputといった関数もありますが、これらは古い仕様の関数を互換性のために残しているものですので、今後はenableXXXの関数を使うようにしてください。

他にPD制御に必要な値として、

q_ref = q_prev = joint->q();

によって初期関節角度を取得し、それを変数q_ref、 q_prevに代入しています。q_refは目標関節角で、q_prevは関節角速度計算用の変数です。また、

dt = io->timeStep();

によって変数dtにタイムステップを代入しています。これはシミュレーションの物理計算１回あたりに進める内部の時間を表していて、この時間間隔で次の control 関数が呼ばれることになります。

最後にinitialize関数の戻り値として true を返して、初期化に成功したことをシステムに伝えています。

control関数の実装 ¶

control関数は実際の制御コードを記述する部分で、シミュレーション中に繰り返し実行されます。

ここでは砲塔ピッチ軸に関するPD制御のコードが書かれているだけです。

static const double P = 200.0;
static const double D = 50.0;

はPゲイン、Dゲインの値で、

double q = joint->q(); // input

によって現在関節角を入力し、

double dq = (q - q_prev) / dt;

によって現在角速度を算出し、

double dq_ref = 0.0;

で目標角速度は0に設定し、

joint->u() = P * (q_ref - q) + D * (dq_ref - dq); // output

によってPD制御で計算したトルク値を関節に出力し、

q_prev = q;

によって次回計算用にq_prevを更新しています。

このように、入出力はLinkオブジェクトの変数を用いて行うことがポイントです。joint->q()、joint->u() はそれぞれ関節角度、関節トルクの変数に対応しています。

最後に、正常終了を表すtrueを戻り値として返しています。これによって制御ループが継続されます。

ファクトリ関数の定義 ¶

シンプルコントローラのクラスを定義したら、そのオブジェクトを生成する「ファクトリ関数」も所定の形式で定義しておく必要があります。これは、シンプルコントローラアイテムが実行時にコントローラの共有ライブラリを読み込んで、そこからコントローラのオブジェクトを生成するために必要となります。

これはマクロを使って、

CNOID_IMPLEMENT_SIMPLE_CONTROLLER_FACTORY(TurretController1)

と記述することができます。引数としてはこのようにコントローラのクラス名を与えて下さい。