状態機械による歩行

前のページ
目次
次のページ

このページの内容

歩行

　状態機械を使用してアイボに歩行させることができます。
　例えば、次図に示すように、「伏せ」→「座り」→「立ち」→「歩行」の順に動かすことが可能です。

　　

　例： 状態機械による歩行プログラム MyStateWalk.h

　状態機械を使用してアイボに歩行させる方法は簡単です。
　歩行ノードは次のように作成します。

　　// 歩行ノードを作成する例
　　WalkNode * move_node = new WalkNode("Move",150,0,0);

　あとは、普通のノードと同じように、歩行ノードを追加して、遷移させればOKです。


　【構文】　歩行ノードを作成する　　
WalkNode * WalkNodeName = new WalkNode( "StateName", dx, dy, da);
WalkNodeName歩行ノード名
StateName状態名
dx前向き速度（ミリ／秒）マイナスでバック。範囲（−100〜+150)
dy左向き速度（ミリ／秒）マイナスで右向き。範囲（-50〜+50）
da回転速度（ラジアン／秒）マイナスで時計回り。範囲（-1〜+1）
#ifndef INCLUDED_MyStateWalk_h_
#define INCLUDED_MyStateWalk_h_

#include "Shared/ERS7Info.h"
#include "Shared/WorldState.h"

#include "StateHeader.h"

// Walkノードを使用したサンプルプログラム
class MyStateWalk : public StateNode {
protected:
	StateNode * start_node; // start_nodeは、クラス全体で使用するのでココに定義する
public:
	// コンストラクタ
	MyStateWalk() : StateNode("MyStateWalk"), start_node(NULL) {}

	// デコンストラクタ
	~MyStateWalk() {
		if(issetup)
			teardown();
	}

virtual void setup() {

	StateNode::setup();

	// 伏せノード作成
	StateNode * lie_node = new SmallMotionSequenceNode("Lie","lielie.mot");
	addNode(lie_node);

	// 座り姿勢ノード
	StateNode * sit_node = new SmallMotionSequenceNode("Sit","sitsit.mot");
	addNode(sit_node);

	// 立ち姿勢ノード
	StateNode * stand_node = new MediumMotionSequenceNode("Stand","sitwalk.mot");
	addNode(stand_node);

	// 頭ニュートラル_ノード
	StateNode * headneutral_node = new SmallMotionSequenceNode("HeadNeutral","headneut.mot");
	addNode(headneutral_node);

	// 前進歩行ノード
	WalkNode * move_node = new WalkNode("Move",150,0,0);
	addNode(move_node);

	// スタートは伏せ姿勢
	start_node = lie_node;

	// 遷移（伏せ→座り→立ち→頭正面）
	lie_node->addTransition(new CompletionTrans(sit_node));
	sit_node->addTransition(new CompletionTrans(stand_node));
	stand_node->addTransition(new CompletionTrans(headneutral_node));

	// 頭正面の状態で１秒経過したら前進歩行を開始
	headneutral_node->addTransition(new TimeOutTrans(move_node,1000));

}

virtual void DoStart() {
	StateNode::DoStart();
	start_node->DoStart();
}

virtual void DoStop() {
	erouter->removeListener(this);
	StateNode::DoStop();
}

virtual void teardown() {
	StateNode::teardown();
}

private:  // コンパイル時の警告を避けるためのダミー関数
	MyStateWalk(const MyStateWalk&);
	MyStateWalk operator=(const MyStateWalk&);
};

#endif

秀丸を開き、上記プログラム（背景が黄色）をコピー＆貼り付け、次の通り保存してください。

　　　　保　存　先　：　マイドキュメント → usXX → project
　　　　ファイル名　：　MyStateWalk.h
　　　　ファイル種類：　C言語ヘッダーファイル（*.h）

ファイルを保存したら、次の手順で実行してください。

実行するとアイボは前進し続けます。
アイボの前に障害物を置かないでください。

追跡

　今度は単に歩行させるだけでなく、物を追いかけさせるプログラムです。
　例えば、ピンクボールを見つけたら、嬉しそうに吠えてボールを追跡し、
　ボールを見失うと、悲しげに鳴いて立ち止まります（次図）。

　　

　例： 追跡プログラム MyStateTarget.h

状態機械を使用してアイボに物体を追跡させる方法は簡単です。
　追跡ノードは次のように作成します。

　　// 追跡ノードの作成例
　　WalkToTargetNode * chase_node = new WalkToTargetNode("Chase", ProjectInterface::visPinkBallSID);

　あとは、普通のノードと同じように、追跡ノードを追加して、遷移させればOKです。


　【構文】 追跡ノードを作成する　　
WalkToTargetNode * ChaseNodeName = new WalkToTargetNode( "StateName", SourceID);
ChaseNodeName追跡ノード名
StateName状態名
SourceIDターゲットのソースID
#ifndef INCLUDED_MyStateTarget_h_
#define INCLUDED_MyStateTarget_h_

#include "Shared/ERS7Info.h"
#include "Shared/WorldState.h"

#include "StateHeader.h"

class MyStateTarget : public StateNode {
protected:
	StateNode * start_node; // start_nodeは、クラス全体で使用するのでココに定義する

public:
	// コンストラクタ
	MyStateTarget() : StateNode("MyStateTarget"), start_node(NULL) {}

	// デコンストラクタ
	~MyStateTarget() {
		if(issetup)
			teardown();
	}

virtual void setup() {
	StateNode::setup();

	// 伏せノード作成
	StateNode * lie_node = new SmallMotionSequenceNode("Lie","lielie.mot");
	addNode(lie_node);

	// 座り姿勢ノード
	StateNode * sit_node = new SmallMotionSequenceNode("Sit","sitsit.mot");
	addNode(sit_node);

	// 立ち姿勢ノード
	StateNode * stand_node = new MediumMotionSequenceNode("Stand","sitwalk.mot");
	addNode(stand_node);

	// 頭ニュートラル_ノード
	StateNode * headneutral_node = new SmallMotionSequenceNode("HeadNeutral","headneut.mot");
	addNode(headneutral_node);

	// ボール追跡
	WalkToTargetNode * chase_node = new WalkToTargetNode("Chase", ProjectInterface::visPinkBallSID);
	addNode(chase_node);

	// 停止
	WalkNode * stop_node = new WalkNode("Stop", 0,0,0);
	addNode(stop_node);

	// スタートは伏せ姿勢
	start_node = lie_node;

	// 遷移（伏せ→座り→立ち→頭正面）
	lie_node->addTransition(new CompletionTrans(sit_node));
	sit_node->addTransition(new CompletionTrans(stand_node));
	stand_node->addTransition(new CompletionTrans(headneutral_node));

	// 何度も使う遷移はこのようにNULLで初期化して定義する
	Transition * tmptrans=NULL;

	// 頭正面の状態でピンクボールを見たら追跡開始
	headneutral_node->addTransition(tmptrans=new VisualTargetTrans(chase_node,ProjectInterface::visPinkBallSID));
	tmptrans->setSound("cutey.wav");

	// 見失ったら停止
	chase_node->addTransition(tmptrans=chase_node->newDefaultLostTrans(stop_node));
	tmptrans->setSound("whimper.wav");

	// 再びピンクボールを見たら追跡開始
	stop_node->addTransition(tmptrans=new VisualTargetTrans(chase_node,ProjectInterface::visPinkBallSID));
	tmptrans->setSound("cutey.wav");
	
	// 音声ファイルを読み込む
	sndman->LoadFile("cutey.wav");
	sndman->LoadFile("whimper.wav");

}

virtual void DoStart() {
	StateNode::DoStart();
	start_node->DoStart();
}

// 音声ファイルをLoadFileで読み込んだときは teardown()で解放する
virtual void teardown() {
	// 音声ファイルを解放する
	sndman->ReleaseFile("cutey.wav");
	sndman->ReleaseFile("whimper.wav");
	StateNode::teardown();
}

private:  // コンパイル時の警告を避けるためのダミー関数
	MyStateTarget(const MyStateTarget&);
	MyStateTarget operator=(const MyStateTarget&);
};

#endif

秀丸を開き、上記プログラム（背景が黄色）をコピー＆貼り付け、次の通り保存してください。

　　　　保　存　先　：　マイドキュメント → usXX → project
　　　　ファイル名　：　MyStateTarget.h
　　　　ファイル種類：　C言語ヘッダーファイル（*.h）

ファイルを保存したら、次の手順で実行してください。

徘徊

　うろうろと歩き回るプログラムを作ります。
　しかし、単に歩き回っていると障害物に衝突してしまいます。
　実はアイボの胸には赤外線による障害物センサが内蔵されており、これをプログラムに組み込むと障害物を検知するようになります。
　ここでは、障害物がないときは前進し、障害物に近づくと方向転換するプログラムを作ります。

　例： 状態機械による徘徊プログラム MyStateExplore.h

#ifndef INCLUDED_MyStateExplore_h_
#define INCLUDED_MyStateExplore_h_

#include "Shared/ERS7Info.h"
#include "Shared/WorldState.h"

#include "StateHeader.h"

// Walkノードを使用したサンプルプログラム
class MyStateExplore : public StateNode {
protected:
	StateNode * start_node; // start_nodeは、クラス全体で使用するのでココに定義する
	WalkNode * turn_node;   // turn_nodeは、クラス全体で使用するのでココに定義する
public:
	// コンストラクタ
	MyStateExplore() : StateNode("MyStateExplore"), start_node(NULL), turn_node(NULL) {}

	// デコンストラクタ
	~MyStateExplore() {
		if(issetup)
			teardown();
	}

virtual void setup() {

	StateNode::setup();

	// 赤外線センサ設定
	unsigned int IRDistOffset;
	IRDistOffset=ERS7Info::NearIRDistOffset;

	// 伏せノード作成
	StateNode * lie_node = new SmallMotionSequenceNode("Lie","lielie.mot");
	addNode(lie_node);

	// 座り姿勢ノード
	StateNode * sit_node = new SmallMotionSequenceNode("Sit","sitsit.mot");
	addNode(sit_node);

	// 立ち姿勢ノード
	StateNode * stand_node = new MediumMotionSequenceNode("Stand","sitwalk.mot");
	addNode(stand_node);

	// 頭ニュートラル_ノード
	StateNode * headneutral_node = new SmallMotionSequenceNode("HeadNeutral","headneut.mot");
	addNode(headneutral_node);

	// 前進歩行ノード
	WalkNode * move_node = new WalkNode("Move",150,0,0);
	addNode(move_node);

	// 旋回ノード追加
	addNode(turn_node = new WalkNode("Turn",0,0,0.5f));

	// スタートは伏せ姿勢
	start_node = lie_node;

	// 遷移（伏せ→座り→立ち→頭正面）
	lie_node->addTransition(new CompletionTrans(sit_node));
	sit_node->addTransition(new CompletionTrans(stand_node));
	stand_node->addTransition(new CompletionTrans(headneutral_node));

	// 頭正面の状態で１秒経過したら前進歩行を開始
	headneutral_node->addTransition(new TimeOutTrans(move_node,1000));

	/* 障害物までの距離が350ミリ以下になったら遷移（前進→旋回） */
	move_node->addTransition(new SmoothCompareTrans<float>(
			turn_node,
			&state->sensors[IRDistOffset],
			CompareTrans<float>::LT,
			350,
			EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID),
			.7));

	/* 旋回の状態で２秒経過したら遷移（旋回→前進） */
	turn_node->addTransition(new TimeOutTrans(move_node,2000));

}

virtual void DoStart() {
	StateNode::DoStart();
	start_node->DoStart();
	erouter->addListener(this,EventBase::stateMachineEGID,(size_t)turn_node,EventBase::activateETID);
}

virtual void DoStop() {
	erouter->removeListener(this);
	StateNode::DoStop();
}

virtual void teardown() {
	StateNode::teardown();
}

// turnノードが呼ばれるたびに、旋回速度をランダムに変更する
virtual void processEvent(const EventBase& /*e*/) {

	float vel=rand()/(float)RAND_MAX*2.0f-1;

	if(vel<0)
		vel-=.25;
	if(vel>0)
		vel+=.25;

	turn_node->setAVelocity(vel);
}

private:  // コンパイル時の警告を避けるためのダミー関数
	MyStateExplore(const MyStateExplore&);
	MyStateExplore operator=(const MyStateExplore&);
};

#endif

　秀丸を開き、上記プログラム（背景が黄色）をコピー＆貼り付け、次の通り保存してください。

　　　　保　存　先　：　マイドキュメント → usXX → project
　　　　ファイル名　：　MyStateExplore.h
　　　　ファイル種類：　C言語ヘッダーファイル（*.h）

　ファイルを保存したら、次の手順で実行してください。


　【構文】　状態をスムースに遷移させる　SmoothCompareTrans　
SmoothCompareTrans (
　　DestinationNode,
　　Monitor,
　　SmoothCompareTrans( <type>::enumerator),
　　value,
　　event,
　　speed
);
DestinationNode次ノード名
Monitorモニタ対象のセンサ
typeデータ型
enumerator比較条件
valueセンサ値
eventイベント
speed遷移速度
　　比較条件:
LTless than （＜）
GTgreater than （＞）
LTEless than or equal （≦）
GTEgreater than or equal （≧）
EQequal （＝）
NEnot equal （≠）

前のページ
目次
次のページ