# パート4: クローラの制御

パート3で砲塔の制御ができるようになりましたので、パート4では車体を動かすクローラ部分の制御をできるようにしましょう。

目次

- Tankモデルの簡易クローラ
- 簡易クローラのコントローラ
- コントローラの導入

## Tankモデルの簡易クローラ

Tankモデルの左右のクローラは「簡易クローラ」としてモデリングされています（クローラの記述 参照）。これはクローラ部と環境との接触点に推力を与えるというもので、履帯の部分が実際にホイールの周りを動いていくというものではないのですが、これを用いることでクローラ風の動きをすることが可能です。ただし、履帯の部分が地形に沿って変形していくようなものではないので、走破性は本物のクローラには及びません。この詳細は 無限軌道の簡易シミュレーション を参照して下さい。

Tankモデルでは、左クローラに対応するリンクが “TRACK_L”、右クローラに対応するリンクが “TRACK_R” という名前でモデリングされています。これらのリンクは簡易クローラに対応する “pseudoContinuousTrack” タイプの軸が設定されており、それらの軸をゲームパッドで制御できるようにしたいと思います。

## 簡易クローラのコントローラ

今回作成するコントローラ “TrackController” のソースコードを以下に示します。

     1
     2
     3
     4
     5
     6
     7
     8
     9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    #include <cnoid/SimpleController>
    #include <cnoid/Joystick>

    using namespace cnoid;

    class TrackController : public SimpleController
    {
        Link* trackL;
        Link* trackR;
        Joystick joystick;

    public:
        virtual bool initialize(SimpleControllerIO* io)
        {
            trackL = io->body()->link("TRACK_L");
            trackR = io->body()->link("TRACK_R");

            io->setLinkOutput(trackL, JOINT_VELOCITY);
            io->setLinkOutput(trackR, JOINT_VELOCITY);

            return true;
        }

        virtual bool control()
        {
            static const int trackAxis[] = { 0, 1 };

            joystick.readCurrentState();

            double pos[2];
            for(int i=0; i < 2; ++i){
                pos[i] = joystick.getPosition(trackAxis[i]);
                if(fabs(pos[i]) < 0.25){
                    pos[i] = 0.0;
                }
            }

            trackL->dq() = -2.0 * pos[1] + pos[0];
            trackR->dq() = -2.0 * pos[1] - pos[0];

            return true;
        }
    };

    CNOID_IMPLEMENT_SIMPLE_CONTROLLER_FACTORY(TrackController)

これまでと同様に、上記のソースコードを “TrackController.cpp” というファイル名でプロジェクトディレクトリに保存し、同ディレクトリ内のCMakeLists.txtに

    add_cnoid_simple_controller(TankTutorial_TrackController TrackController.cpp)

という記述を追加し、コンパイルを行って下さい。

## コントローラの導入

コントローラの導入についても、これまでと同様に…と言いたいところですが、今回作成したコントローラは砲塔を制御する部分がありませんので、このコントローラだけセットしても、砲塔を動かすことができなくなってしまいます。そこで、これまでの砲塔のコントローラを生かしつつ、今回のコントローラを追加で導入したいと思います。

注釈

もちろん、今回のソースコードに砲塔制御のコードも統合しておくという手もあります。その場合はこれまでと同様に、シンプルコントローラアイテムの「コントローラモジュール」に統合したコントローラのファイルを指定すればOKです。本チュートリアルではパートごとに分けて説明する上で分かりやすくするため、各パートのコントローラはそれに特化したものとしています。