コントローラの実装

コントローラの実装

ここでは、コントローラの実装方法について、その基本を解説します。

コントローラが行うことは、基本的には以下の3つで、これらを「制御ループ」として繰り返し実行します。

  1. ロボットの状態を入力
  2. 制御計算
  3. ロボットへ指令を出力

これらの処理を単体のコントローラで行うこともあれば、複数のソフトウェアコンポーネントを組み合わせて行うこともあります。また、「制御計算」とひとくくりにした処理には、実際には各種認識・動作計画等の多様な処理がからむものですし、ロボット以外を対象とした入出力も含まれる可能性があります。しかし、ロボットを中心としてみると、最終的にコントローラがやっていることは上記の3つの処理に整理して考えることができます。

このように考えると、コントローラというものは、上記3つを行うためのインタフェースを備えたソフトウェアモジュールであると言えます。そのための実際のAPIはコントローラの形式によって異なってくるわけですが、本質的な部分は同じです。

以下では コントローラの導入 でも用いた "SR1MinimumController"サンプルを通して解説を行います。コントローラの形式はChoreonoidのサンプル用に設計された「シンプルコントローラ」形式であり、制御の内容は関節に対するPD制御でロボットの姿勢を維持するというだけのものです。記述言語はC++です。

実際にコントローラ開発する際には、このサンプルを通して述べた基本的な事柄を希望のコントローラ形式や制御内容に置き換えて、取り組んでいただければよいかと思います。一般的に、ロボットのコントローラ開発では制御やプログラミング、ハードウェア等に関する様々な知識とスキルが必要となります。それらの多くについては本マニュアルの対象外となりますので、やりたいことに応じて、別途取り組むようにしてください。

サンプルコントローラのソースコード

まず、SR1MinimumControllerのソースコードを以下に示します。本ソースコードはChoreonoidソースの"sample/SimpleController"ディレクトリ内にある"SR1MinimumController.cpp"というファイルです。

#include <cnoid/SimpleController>
#include <vector>

using namespace cnoid;

const double pgain[] = {
    8000.0, 8000.0, 8000.0, 8000.0, 8000.0, 8000.0,
    3000.0, 3000.0, 3000.0, 3000.0, 3000.0, 3000.0, 3000.0,
    8000.0, 8000.0, 8000.0, 8000.0, 8000.0, 8000.0,
    3000.0, 3000.0, 3000.0, 3000.0, 3000.0, 3000.0, 3000.0,
    8000.0, 8000.0, 8000.0 };

const double dgain[] = {
    100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0 };

class SR1MinimumController : public SimpleController
{
    BodyPtr ioBody;
    double dt;
    std::vector<double> qref;
    std::vector<double> qold;

public:

    virtual bool initialize(SimpleControllerIO* io) override
    {
        ioBody = io->body();
        dt = io->timeStep();

        for(int i=0; i < ioBody->numJoints(); ++i){
            Link* joint = ioBody->joint(i);
            joint->setActuationMode(Link::JointTorque);
            io->enableIO(joint);
            qref.push_back(joint->q());
        }
        qold = qref;

        return true;
    }

    virtual bool control() override
    {
        for(int i=0; i < ioBody->numJoints(); ++i){
            Link* joint = ioBody->joint(i);
            double q = joint->q();
            double dq = (q - qold[i]) / dt;
            double u = (qref[i] - q) * pgain[i] + (0.0 - dq) * dgain[i];
            qold[i] = q;
            joint->u() = u;
        }
        return true;
    }
};

CNOID_IMPLEMENT_SIMPLE_CONTROLLER_FACTORY(SR1MinimumController)

このコントローラはChoreonoidに付属のサンプルであり、デフォルトでChoreonoid本体と一緒にビルドされるようになっています。(CMakeの設定で BUILD_SIMPLE_CONTROLLER_SAMPLES がONになっていればOKです。)

サンプルとは別に新たにシンプルコントローラを実装してビルドする方法については、コントローラのビルド をご参照ください。

SimpleControllerクラス

シンプルコントローラ形式のコントローラは、SimpleControllerクラスを継承することで実装します。このクラスは

#include <cnoid/SimpleController>

により、cnoid/SimpleControllerヘッダをインクルードすることで使えるようになります。

このクラスは基本的には以下のような定義になっています。

class SimpleController
{
public:
    virtual bool initialize(SimpleControllerIO* io);
    virtual bool control();

};

このクラスのvirtual関数を継承先のクラスでオーバーライドすることにより、コントローラの処理内容を記述します。各関数の内容は以下のようになっています。

  • virtual bool initialize(SimpleControllerIO* io)
コントローラの初期化処理を行います。引数 io を通して制御に関わるオブジェクトや情報を取得できます。
  • virtual bool control()
コントローラの入力・制御・出力処理を行います。制御中この関数は制御ループとして繰り返し実行されることになります。

SimpleControllerを継承したクラスを定義したら、そのファクトリ関数を定義しておく必要があります。これは以下のようにマクロを用いて記述すればOKです。

CNOID_IMPLEMENT_SIMPLE_CONTROLLER_FACTORY(SR1MinimumController)

これにより、このソースからコンパイルされた共有(ダイナミックリンク)ライブラリのファイルが、実際のコントローラとしてシンプルコントローラアイテムから利用可能となります。

注釈

SimpleControllerクラスの詳細については、ソースアーカイブにおいてこのクラスを定義している "src/SimpleControllerPlugin/library/SimpleController.h" をご参照ください。また本節最後の 補足: SimpleControllerクラスの定義について も参考にしてください。

IOオブジェクト

上記のinitialize関数の引数 io として渡されるSimpleControllerIO型のオブジェクトは、コントローラとロボットの間の入出力に必要な情報を扱うオブジェクトです。以下ではこのオブジェクトを「IOオブジェクト」と呼ぶことにします。

このクラスはControllerIOを継承したものになっています。ControllerIOクラスで定義されている関数としては以下のようなものがあり、コントローラの実装に用いることができます。

  • Body* controllerName()
コントローラの名前を返します。
  • Body* body()
入出力に使うためのBodyオブジェクトを返します。
  • std::string optionString() const
コントローラに与えられたオプション文字列を返します。
  • std::vector<std::string> options() const
オプション文字列についてスペースで区切って分解したものを返します。
  • std::ostream& os() const
コントローラからのメッセージを出力する出力ストリームを返します。
  • double timeStep() const
タイムステップを返します。単位は秒です。
  • double currentTime() const
現在時刻を返します。単位は秒で、シミュレーション開始時が時刻 0 となります。

Bodyオブジェクトを介した入出力

シンプルコントローラでは、「Bodyオブジェクト」を介して入出力を行います。Bodyオブジェクトは、 ボディモデル のChoreonoid内部での表現で、C++で定義された「Bodyクラス」のインスタンスです。Bodyクラスはロボットのモデルとその状態を格納するためのデータ構造なので、入出力対象となる関節角度やトルク、センサの状態に関する値も格納できます。そこで、シンプルコントローラではこのBodyオブジェクトを介して入出力を行うこととしています。このためのBodyオブジェクトはIOオブジェクトのbody関数で取得することが可能です。

状態変数シンボル

Choreonoidでは入出力の対象となる状態変数を識別するためのシンボルが定義されており、これを用いて指令値や入力値にどの状態変数を使用するかを指定します。シンボルはLinkクラスにてStateFlagという列挙型の要素として以下のように定義されています。(外部からはLinkクラスのスコープ解決演算子 Link:: を付けてアクセスできます。)

Link::StateFlag列挙型のシンボル
シンボル 内容 対応する状態変数
StateNone 該当する状態なし。  
JointEffort 関節に加わるトルク(回転関節の場合)または力(直動関節の場合) Link::u()
JointTorque JointEffortと同じ。回転関節向けに記述を分かりやすくするために定義されている。 同上
JointForce JointEffortと同じ。直動関節向けに記述を分かりやすくするために定義されている。 同上
JointDisplacement 関節変位(関節角度や関節並進位置) Link::q() (現在値)またはLink::q_target() (指令値)
JointAngle JointDisplacementと同じ。対応する変位が関節角度である場合に記述をより分かりやすくするために定義されている。 同上
JointVelocity 関節の速度成分。回転関節の角速度または直動関節の変位速度に該当。 Link::dq() (現在地)またはLink::dq_target() (指令値)
JointAcceleration 関節の加速度成分。回転関節の角加速度または直動関節の変位加速度に該当。 Link::ddq()
LinkPosition リンク位置。デカルト空間におけるリンク座標フレームの6自由度の位置・姿勢に該当。 Link::T()
LinkTwist リンク速度。リンク座標フレームの並進速度・角速度。 Link::v() (並進速度)、Link::w() (角速度)

複数の要素を組み合わせることも可能です。その場合は複数シンボルをビット演算子の '|' で列挙します。例えば、

JointDisplacement | JointVelocity

と指定することで、関節変位と関節速度の両方を指定することができます。

注釈

Choreonoid 1.7までのシンボルは大文字とアンダースコアを組み合わせた "JOINT_EFFORT" といった形式でしたが、Choreonoid 1.8からは上記の形式のシンボルとなりました。古いシンボルもしばらくは使用できますが、今後は新しいシンボルを使用するようにしてください。

アクチュエーションモード

コントローラから各リンク/関節への出力に関わる概念として、「アクチュエーションモード」があります。これはどの状態変数を制御指令値として使用するかを決めるものです。モードの指定には上記のStateFlag列挙型のシンボルを用います。

基本的な関節指令値に対応するモードは以下のようになります。

基本的なアクチュエーションモード
モード 内容 状態変数
StateNone 駆動なし。関節はフリーの状態となる。  
JointEffort 関節を駆動する力/トルクを指令値とする。 Link::u()
JointDisplacement 関節変位を指令値とする。 Link::q_target()
JointVelocity 関節の角速度や変位速度を指令値とする。 Link::dq_target()

アクチュエーションモードは、Linkクラスの以下の関数を用いて参照・設定します。

  • void setActuationMode(int mode)
アクチュエーションモードを設定します。モード値modeはLink::StateFlagのシンボルで指定します。複数のシンボルをビット和として組み合わせて指定することも可能です。
  • int actuationMode() const
現在設定されているアクチュエーションモードを返します。値は通常Link::StateFlagの要素ひとつとなりますが、複数要素の組み合わせ(ビット集合)の場合もあります。

入出力の有効化

コントローラからどの状態変数の入出力を行うかについては、IOオブジェクトを用いて設定します。SimpleControllerIOクラスにはこれを行うための以下の関数が定義されています。

  • void enableInput(Link* link)
指定したリンク(関節)が有する状態量のコントローラへの入力を有効にします。リンクに対して設定されているアクチュエーションモードに対して適切な状態量が入力対象となります。例えばアクチュエーションモードとしてJointEffortが設定されている場合は、関節変位の現在値であるLink::q()が入力対象となります。これはPD制御を行うために必要となるからです。
  • void enableInput(Link* link, int stateFlags)
指定したリンク(関節)が有する状態量のうちstateFlagsで指定したものをコントローラに入力するようにします。stateFlagsはLink::StateFlagシンボルの論理和で指定します。入力したい値が明確な場合はこの関数を使用するのがよいです。
  • void enableOutput(Link* link)
指定したリンク(関節)に対する出力を有効にします。リンクに対して設定されているアクチュエーションモードに対応する状態変数が出力対象となります。例えばアクチュエーションモードとしてJointEffortが設定されている場合は、関節トルク/力に対応するLink::u()が出力対象となります。
  • void enableOutput(Link* link, int stateFlags)
指定したリンク(関節)に対する出力を有効にします。出力する状態変数はstateFlagsにLink::StateFlagのシンボルを指定することで行います。
  • void enableIO(Link* link)
指定したリンクの入出力を有効にします。リンクに対して設定されているアクチュエーションモードに対して適切な状態量が入出力対象となります。

注釈

SimpleControllerIO には setLinkInput、setJointInput、setLinkOutput、setJointOutput といった関数も定義されています。これらはChoroenoid 1.5以前のバージョンで使われていた関数ですが、バージョン1.6以降ではこれらの関数に代わるものとして上記の enableIO、enableInput、enableOutput 関数を導入されており、今後はそちらの関数を使うようにしてください。

実際に利用可能なアクチュエーションモードは、シミュレータアイテム(≒物理エンジン)のタイプや設定によって変わってきます。ほとんどのシミュレータアイテムではJOINT_EFFORTに対応しており、これとJOINT_DISPLACEMENTの入力を組み合わせることで、PD制御等を行うことが可能です。

Linkオブジェクトに設定されているアクチュエーションモードに対して、入出力対象は通常以下のようになります。

アクチュエーションモード 入力 出力
JointEffort Link::q() Link::u()
JointDisplacement_DISPLACEMENT なし Link::q_target()
JointVelocity Link::q() Link::dq_target()

注釈

LinkPosition を指定することで、3次元空間中のリンクの位置と姿勢を直接入出力の対象とすることも可能です。これについては後ほど リンク位置姿勢の入出力 にて解説します。

初期化処理

SimpleController継承クラスのinitialize関数では、コントローラの初期化を行います。

サンプルでは、まず

ioBody = io->body();

によって、入出力用のBodyオブジェクトを取得し、メンバ変数ioBodyに格納しています。これにより、このオブジェクトをコントローラの他の関数内でも使えるようにしています。

同様に、制御計算で必要となるタイムステップ(デルタタイム)値について、

dt = io->timeStep();

によって値をdtというメンバ変数に格納しています。

次に、以下のfor文でロボットの全関節に対してループを回して初期化の処理を行っています。

for(int i=0; i < ioBody->numJoints(); ++i){
    ...
}

まずこのループの中の

Link* joint = ioBody->joint(i);

によってi番目の関節に対応するリンクオブジェクトを取得し、変数jointに設定しています。

そして

joint->setActuationMode(Link::JointTorque);

によって、この関節に対してアクチュエーションモードの設定を行っています。ここでは Link::JointTorque を指定することで、関節トルクを指令値としています。また、

io->enableIO(joint);

とすることで、この関節に対する入出力を有効化しています。アクチュエーションモードに JointTorque が設定されているため、出力は関節トルク、入力は関節角度となります。これによってPD制御を行います。

次に

qref.push_back(joint->q());

によってロボットの初期状態における関節角度をベクタ変数qrefに格納しています。こちらもPD制御で用います。ここで各関節に対するforループを終了します。

次に

qold = qref;

によってベクタ変数qoldをqrefと同じ値で初期化しています。これはPD制御において1ステップ前の関節角度を参照するための変数となります。

最後に、initialize関数の戻り値としてtrueを返すことで、初期化に成功したことをシミュレータに伝えます。

制御ループ

SimpleController継承クラスでは、そのcontrol関数に制御ループを記述します。

初期化の時と同様に、以下のfor文

for(int i=0; i < ioBody->numJoints(); ++i){
    Link* joint = ioBody->joint(i);
    ...
}

により、全ての関節に対して制御計算を行っています。この中身が各関節に対する処理コードです。

まず、 現在の関節角度の入力を行います。

double q = joint->q();

PD制御によって関節トルクの指令値を計算します。まず、制御ループの前回の関節角度との差分から、関節角速度を算出します。

double dq = (q - qold[i]) / dt;

制御の目標は初期姿勢の維持ですので、関節角度は初期関節角度、角速度は0(静止状態)を目標として、トルク指令値を計算します。

double u = (qref[i] - q) * pgain[i] + (0.0 - dq) * dgain[i];

ソースの冒頭で設定したpgain, dgainの配列から、各関節に関するゲイン値を取り出しています。ゲイン値についてはモデルごとに調整が必要ですが、その方法についてはここでは割愛します。

次回の計算用に、関節角度をqold変数に保存しておきます。

qold[i] = q;

計算したトルク指令値を出力します。これにより、関節が初期関節角度を維持するように制御されます。

joint->u() = u;

以上が全ての関節に対して適用されることにより、ロボット全体の姿勢も維持されることになります。

最後にこのcontrol関数がtrueを返すことで、制御が継続している旨をシミュレータに伝えています。これにより、control関数が繰り返し呼ばれることになります。

デバイスに対する入出力

デバイスとは

これまでは入出力の対象として、関節角度や関節トルクといった関節に関わる状態量への入出力を扱いました。一方で、関節とは独立した入出力要素もあります。Choreonoidではそれらを「デバイス」として定義しており、Bodyモデルの構成要素となります。

デバイスの例としては、まず

  • 力センサ、加速度センサ、角速度センサ(レートジャイロ)
  • カメラ、レーザーレンジセンサ

といったデバイスが挙げられます。これらはセンサとして主に入力の対象となるものです。

また、主に出力の対象として外界に働きかけるものとして、

  • ライト
  • スピーカ
  • ディスプレイ

といったデバイスもあり得ます。(スピーカ、ディスプレイは例として挙げただけでまだ実装されていません。)

実際のコントローラ開発においては、これらの多様なデバイスに対しても入出力を行う必要が出てきます。これを行うためには、

  • モデルにおいてデバイスがどのように定義されているか
  • 使用するコントローラ形式において所定のデバイスにどのようにアクセスするか

を把握している必要があります。

デバイスオブジェクト

Choreonoidのボディモデルにおいて、デバイスの情報は「Deviceオブジェクト」として表現されます。これは「Deviceクラス」を継承した型のインスタンスで、デバイスの種類ごとにそれぞれ対応する型が定義されています。標準で定義されている主なデバイス型は以下のようになっています。

+ Device
  + ForceSensor (力センサ)
  + RateGyroSensor (角速度センサ)
  + AccelerationSensor (加速度センサ)
  + Camera (カメラ)
    + RangeCamera (カメラ+距離画像センサ)
  + RangeSensor (レンジセンサ)
  + Light
    + PointLight (点光源ライト)
    + SpotLight (スポットライト)

ロボットに搭載されているデバイスの情報は、通常はモデルファイルにおいて記述します。標準形式のモデルファイルでは、 Bodyファイル リファレンスマニュアル各種センサ・デバイスを定義するノード を記述します。

シンプルコントローラでは、Body、Linkオブジェクトと同様に、デバイスに対してもChoreonoidの内部表現であるDeviceオブジェクトをそのまま用いて入出力を行います。

本節で使用しているSR1モデルが有するデバイスオブジェクトは以下のようになっています。

名前 デバイスの型 内容
WaistAccelSensor AccelerationSensor 腰リンクに搭載された加速度センサ
WaistGyro RateGyroSensor 腰リンクに搭載されたジャイロ
LeftCamera RangeCamera 左目に対応する距離画像センサ
RightCamera RangeCamera 右目に対応する距離画像センサ
LeftAnkleForceSensor ForceSensor 左足首に搭載された力センサ
RightAnkleForceSensor ForceSensor 右足首に搭載された力センサ

デバイスオブジェクトの取得

DeviceオブジェクトはBodyオブジェクトから以下のメンバ関数を用いて取得できます。

  • int numDevices() const
デバイスの数を返します。
  • Device* device(int i) const
i番目のデバイスを返します。デバイスの順番はモデルファイル中の記述順になります。
  • const DeviceList<>& devices() const
全デバイスのリストを返します。
  • template<class DeviceType> DeviceList<DeviceType> devices() const
指定した型のデバイスのリストを返します。
  • template<class DeviceType> DeviceType* findDevice(const std::string& name) const
指定した型と名前を有するデバイスがあればそれを返します。

特定の型のデバイスを取得するには、テンプレートクラスDeviceListを使用します。DeviceListは指定した型のデバイスオブジェクトを格納する配列であり、そのコンストラクタや抽出オペレータ(<<)等を用いて、他の型も含むDeviceListから対応する型のみを抽出できます。例えばBodyオブジェクト"ioBody"の保有する力センサを取得したい場合は、

DeviceList<ForceSensor> forceSensors(ioBody->devices());

としてもよいですし、既存のリストに対して

forceSensors << ioBody->devices();

として追加することもできます。

DeviceListはstd::vectorと同様の関数や演算子を備えており、例えば

for(size_t i=0; i < forceSensors.size(); ++i){
    ForceSensor* forceSensor = forceSensor[i];
    ...
}

といったかたちで各オブジェクトにアクセスできます。

findDevice関数を用いることで、型と名前でデバイスを特定して取得することもできます。例えばSR1モデルは腰リンクに搭載された "WaistAccelSensor" という名前の加速度センサを有しています。これを取得するには、Bodyオブジェクトに対して

AccelerationSensor* accelSensor =
    ioBody->findDevice<AccelerationSensor>("WaistAccelSensor");

などとすればOKです。

入出力方法

Deviceオブジェクトを介した入出力は、以下のようにして行います。

  • 入力

シンプルコントローラのIOオブジェクトに対して関数

  • void enableInput(Device* device)

を実行し、デバイスへの入力を有効にしておく。その上で、対応するDeviceオブジェクトのメンバ関数を用いて値を取得する。

  • 出力

対応するDeviceオブジェクトのメンバ関数を用いて値を設定した後、Deviceオブジェクトの関数

  • void notifyStateChange()

を実行し、デバイスの状態の更新をシミュレータに伝える。

これらを行うためには、使用するデバイスのクラス定義を知っている必要があります。例えば加速度センサのクラスである"AccelerationSensor"に関しては、その状態にアクセスするための"dv()"というメンバ関数があります。これは加速度をVector3型の3次元ベクトルで返します。

SR1モデルの加速度センサの入力は以下のような流れになります。まずコントローラの initialize 関数で

AccelerationSensor* accelSensor =
    ioBody->findDevice<AccelerationSensor>("WaistAccelSensor");
io->enableInput(accelSensor);

などとして、accelSensorへの入力を有効化しておきます。そして、control関数内で加速度センサの値を参照した箇所で

Vector3 dv = waistAccelSensor->dv();

といったかたちで取得することができます。

同様に、ForceSensorやRateGyroSensorに関しても該当するメンバ関数を用いて状態の入力を行うことが可能です。

カメラやレンジセンサ等の視覚センサを使用する際には、そのための準備が必要になります。これについては 視覚センサのシミュレーション で解説します。

デバイスへの出力については、ライトのオン・オフを行う "TankJoystickLight.cnoid" というサンプルを参考にしてください。

補足: SimpleControllerクラスの定義について

SimpleControllerクラス ではこのクラスのvirtual関数として initialize と control の2つを紹介しましたが、SimpleControllerはこれ以外にも以下に示すvirtual関数を備えていて、それぞれオーバーライドして処理を記述できるようになっています。

  • virtual bool configure(SimpleControllerConfig* config)
こちらもコントローラの初期化を行うための関数ですが、initialize関数とは実行のタイミングが異なります。initialize関数はシミュレーションを開始するタイミングで実行されますが、こちらの関数はそれ以前に、コントローラがプロジェクトに導入されて(アイテムツリーに組み込まれて)特定のモデルと関連付けられた時点で実行されます。シミュレーション開始前に実行しておきたい処理がある場合は、こちらに記述するようにします。引数configを通して初期化に関わる情報を取得できます。
  • virtual bool start()
こちらもコントローラの初期化を行うための関数ですが、タイミング的にはinitialize関数よりも後に実行される関数です。initialize関数も含めてシミュレーション全体の初期化がひととおり完了し、コントローラが稼働を開始するタイミングで実行されます。
  • virtual void stop()
こちらはシミュレーション停止時に実行される関数です。
  • virtual void unconfigure()
こちらはconfigureと対をなすもので、コントローラが削除されたり対象モデルから切り離されるなどして、コントローラが無効となるタイミングで実行されます。

configure関数の引数として与えられるconfigオブジェクトは IOオブジェクト と同様のもので、対象モデルの情報を得るための関数

  • Body* body()

をはじめとして、IOオブジェクトと同様のメンバを有しています。

ただしconfigから得られるbodyオブジェクトはIOオブジェクトから得られるものとは異なることにご注意ください。IOオブジェクトから得られるものはシミュレーション中のBodyオブジェクトとの入出力を行うもので、シミュレーション実行時に生成されるものです。一方、configから得られるものは、Bodyアイテムが元々有するBodyオブジェクトで、シミュレーションを開始する前から存在するものです。

その他のサンプル

Choreonoidでは、SR1MinimumController以外にも様々なコントローラのサンプルを用意しています。それらを用いたプロジェクトが サンプルプロジェクト に挙げてありますので、参考にしてください。