パート4: クローラの制御¶
パート3で砲塔の制御ができるようになりましたので、パート4では車体を動かすクローラ部分の制御をできるようにしましょう。
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です。本チュートリアルではパートごとに分けて説明する上で分かりやすくするため、各パートのコントローラはそれに特化したものとしています。